diff --git a/libraries/AP_SerialManager/AP_SerialManager.cpp b/libraries/AP_SerialManager/AP_SerialManager.cpp index bc717a99fb..7e26978904 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.cpp +++ b/libraries/AP_SerialManager/AP_SerialManager.cpp @@ -439,6 +439,8 @@ void AP_SerialManager::init() for (uint8_t i=1; inext) { + if (protocol_match(protocol, (enum SerialProtocol)p->state.protocol.get())) { + if (found_instance == instance) { + return &p->state; + } + found_instance++; + } + } +#endif + // if we got this far we did not find the uart return nullptr; } @@ -602,10 +615,23 @@ AP_HAL::UARTDriver *AP_SerialManager::find_serial(enum SerialProtocol protocol, if (_state == nullptr) { return nullptr; } - const uint8_t serial_idx = _state - &state[0]; + const uint8_t serial_idx = _state->idx; // set options before any user does begin() AP_HAL::UARTDriver *port = hal.serial(serial_idx); + +#if AP_SERIALMANAGER_REGISTER_ENABLED + if (port == nullptr) { + // look for a registered port + for (auto p = registered_ports; p; p = p->next) { + if (p->state.idx == serial_idx) { + port = p; + break; + } + } + } +#endif + if (port) { port->set_options(_state->options); } @@ -637,7 +663,7 @@ int8_t AP_SerialManager::find_portnum(enum SerialProtocol protocol, uint8_t inst if (_state == nullptr) { return -1; } - return int8_t(_state - &state[0]); + return int8_t(_state->idx); } // get_serial_by_id - gets serial by serial id @@ -646,6 +672,13 @@ AP_HAL::UARTDriver *AP_SerialManager::get_serial_by_id(uint8_t id) if (id < SERIALMANAGER_NUM_PORTS) { return hal.serial(id); } +#if AP_SERIALMANAGER_REGISTER_ENABLED + for (auto p = registered_ports; p; p = p->next) { + if (p->state.idx == id) { + return (AP_HAL::UARTDriver *)p; + } + } +#endif return nullptr; } @@ -755,6 +788,17 @@ void AP_SerialManager::set_protocol_and_baud(uint8_t sernum, enum SerialProtocol } } +#if AP_SERIALMANAGER_REGISTER_ENABLED +/* + register an external network port. It is up to the caller to use a unique id field + using AP_SERIALMANAGER_NET_PORT_1 as the base id for NET_P1_* + */ +void AP_SerialManager::register_port(RegisteredPort *port) +{ + port->next = registered_ports; + registered_ports = port; +} +#endif // AP_SERIALMANAGER_REGISTER_ENABLED namespace AP { diff --git a/libraries/AP_SerialManager/AP_SerialManager.h b/libraries/AP_SerialManager/AP_SerialManager.h index e3673c6183..4d8bfb66cb 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.h +++ b/libraries/AP_SerialManager/AP_SerialManager.h @@ -23,6 +23,7 @@ #include #include +#include #ifdef HAL_UART_NUM_SERIAL_PORTS #if HAL_UART_NUM_SERIAL_PORTS >= 4 @@ -150,6 +151,13 @@ #define AP_SERIALMANAGER_MSP_BUFSIZE_TX 256 #define AP_SERIALMANAGER_MSP_BAUD 115200 +#ifndef AP_SERIALMANAGER_REGISTER_ENABLED +#define AP_SERIALMANAGER_REGISTER_ENABLED AP_NETWORKING_ENABLED +#endif + +// serial ports registered by AP_Networking will use IDs starting at 21 for the first port +#define AP_SERIALMANAGER_NET_PORT_1 21 // NET_P1_* + class AP_SerialManager { public: AP_SerialManager(); @@ -268,10 +276,12 @@ public: AP_SerialManager::SerialProtocol get_protocol() const { return AP_SerialManager::SerialProtocol(protocol.get()); } - private: AP_Int32 baud; AP_Int16 options; AP_Int8 protocol; + + // serial index number + uint8_t idx; }; // search through managed serial connections looking for the @@ -282,6 +292,24 @@ public: const UARTState *find_protocol_instance(enum SerialProtocol protocol, uint8_t instance) const; +#if AP_SERIALMANAGER_REGISTER_ENABLED + /* + a class for a externally registered port + used by AP_Networking + */ + class RegisteredPort : public AP_HAL::UARTDriver { + public: + RegisteredPort *next; + UARTState state; + }; + RegisteredPort *registered_ports; + + // register an externally managed port + void register_port(RegisteredPort *port); + +#endif // AP_SERIALMANAGER_REGISTER_ENABLED + + private: static AP_SerialManager *_singleton;