mirror of https://github.com/ArduPilot/ardupilot
AP_SerialManager: ensure registered ports are in sorted order
this avoids ordering issues if using both CAN and network serial ports
This commit is contained in:
parent
0ae5c9116c
commit
e63a2250ca
|
@ -850,8 +850,24 @@ void AP_SerialManager::set_protocol_and_baud(uint8_t sernum, enum SerialProtocol
|
||||||
*/
|
*/
|
||||||
void AP_SerialManager::register_port(RegisteredPort *port)
|
void AP_SerialManager::register_port(RegisteredPort *port)
|
||||||
{
|
{
|
||||||
|
const auto idx = port->state.idx;
|
||||||
|
WITH_SEMAPHORE(port_sem);
|
||||||
|
/*
|
||||||
|
maintain the list in ID order
|
||||||
|
*/
|
||||||
|
if (registered_ports == nullptr ||
|
||||||
|
registered_ports->state.idx >= idx) {
|
||||||
port->next = registered_ports;
|
port->next = registered_ports;
|
||||||
registered_ports = port;
|
registered_ports = port;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
for (auto p = registered_ports; p; p = p->next) {
|
||||||
|
if (p->next == nullptr || p->next->state.idx >= idx) {
|
||||||
|
port->next = p->next;
|
||||||
|
p->next = port;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
#endif // AP_SERIALMANAGER_REGISTER_ENABLED
|
#endif // AP_SERIALMANAGER_REGISTER_ENABLED
|
||||||
|
|
||||||
|
|
|
@ -172,6 +172,7 @@ public:
|
||||||
UARTState state;
|
UARTState state;
|
||||||
};
|
};
|
||||||
RegisteredPort *registered_ports;
|
RegisteredPort *registered_ports;
|
||||||
|
HAL_Semaphore port_sem;
|
||||||
|
|
||||||
// register an externally managed port
|
// register an externally managed port
|
||||||
void register_port(RegisteredPort *port);
|
void register_port(RegisteredPort *port);
|
||||||
|
|
Loading…
Reference in New Issue