diff --git a/libraries/AP_SerialManager/AP_SerialManager.cpp b/libraries/AP_SerialManager/AP_SerialManager.cpp index ad8a3b8c40..dba357879d 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.cpp +++ b/libraries/AP_SerialManager/AP_SerialManager.cpp @@ -462,7 +462,7 @@ void AP_SerialManager::init() case SerialProtocol_MAVLink: case SerialProtocol_MAVLink2: case SerialProtocol_MAVLinkHL: - uart->begin(map_baudrate(state[i].baud), + uart->begin(state[i].baudrate(), AP_SERIALMANAGER_MAVLINK_BUFSIZE_RX, AP_SERIALMANAGER_MAVLINK_BUFSIZE_TX); break; @@ -479,7 +479,7 @@ void AP_SerialManager::init() break; case SerialProtocol_GPS: case SerialProtocol_GPS2: - uart->begin(map_baudrate(state[i].baud), + uart->begin(state[i].baudrate(), AP_SERIALMANAGER_GPS_BUFSIZE_RX, AP_SERIALMANAGER_GPS_BUFSIZE_TX); break; @@ -493,7 +493,7 @@ void AP_SerialManager::init() case SerialProtocol_SToRM32: // Note baudrate is hardcoded to 115200 state[i].baud = AP_SERIALMANAGER_SToRM32_BAUD / 1000; // update baud param in case user looks at it - uart->begin(map_baudrate(state[i].baud), + uart->begin(state[i].baudrate(), AP_SERIALMANAGER_SToRM32_BUFSIZE_RX, AP_SERIALMANAGER_SToRM32_BUFSIZE_TX); break; @@ -503,7 +503,7 @@ void AP_SerialManager::init() case SerialProtocol_Volz: // Note baudrate is hardcoded to 115200 state[i].baud = AP_SERIALMANAGER_VOLZ_BAUD; // update baud param in case user looks at it - uart->begin(map_baudrate(state[i].baud), + uart->begin(state[i].baudrate(), AP_SERIALMANAGER_VOLZ_BUFSIZE_RX, AP_SERIALMANAGER_VOLZ_BUFSIZE_TX); uart->set_unbuffered_writes(true); @@ -511,7 +511,7 @@ void AP_SerialManager::init() break; case SerialProtocol_Sbus1: state[i].baud = AP_SERIALMANAGER_SBUS1_BAUD / 1000; // update baud param in case user looks at it - uart->begin(map_baudrate(state[i].baud), + uart->begin(state[i].baudrate(), AP_SERIALMANAGER_SBUS1_BUFSIZE_RX, AP_SERIALMANAGER_SBUS1_BUFSIZE_TX); uart->configure_parity(2); // enable even parity @@ -523,12 +523,12 @@ void AP_SerialManager::init() case SerialProtocol_ESCTelemetry: // ESC telemetry protocol from BLHeli32 ESCs. Note that baudrate is hardcoded to 115200 state[i].baud = 115200 / 1000; - uart->begin(map_baudrate(state[i].baud), 30, 30); + uart->begin(state[i].baudrate(), 30, 30); uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); break; case SerialProtocol_Robotis: - uart->begin(map_baudrate(state[i].baud), + uart->begin(state[i].baudrate(), AP_SERIALMANAGER_ROBOTIS_BUFSIZE_RX, AP_SERIALMANAGER_ROBOTIS_BUFSIZE_TX); uart->set_unbuffered_writes(true); @@ -536,7 +536,7 @@ void AP_SerialManager::init() break; case SerialProtocol_SLCAN: - uart->begin(map_baudrate(state[i].baud), + uart->begin(state[i].baudrate(), AP_SERIALMANAGER_SLCAN_BUFSIZE_RX, AP_SERIALMANAGER_SLCAN_BUFSIZE_TX); break; @@ -549,7 +549,7 @@ void AP_SerialManager::init() case SerialProtocol_EFI: state[i].baud.set_default(AP_SERIALMANAGER_EFI_MS_BAUD); - uart->begin(map_baudrate(state[i].baud), + uart->begin(state[i].baudrate(), AP_SERIALMANAGER_EFI_MS_BUFSIZE_RX, AP_SERIALMANAGER_EFI_MS_BUFSIZE_TX); uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); @@ -563,7 +563,7 @@ void AP_SerialManager::init() case SerialProtocol_MSP_DisplayPort: // baudrate defaults to 115200 state[i].baud.set_default(AP_SERIALMANAGER_MSP_BAUD/1000); - uart->begin(map_baudrate(state[i].baud), + uart->begin(state[i].baudrate(), AP_SERIALMANAGER_MSP_BUFSIZE_RX, AP_SERIALMANAGER_MSP_BUFSIZE_TX); uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); @@ -571,7 +571,7 @@ void AP_SerialManager::init() break; #endif default: - uart->begin(map_baudrate(state[i].baud)); + uart->begin(state[i].baudrate()); } } } @@ -630,7 +630,7 @@ uint32_t AP_SerialManager::find_baudrate(enum SerialProtocol protocol, uint8_t i if (_state == nullptr) { return 0; } - return map_baudrate(_state->baud); + return state->baudrate(); } // find_portnum - find port number (SERIALn index) for a protocol and instance, -1 for not found @@ -643,35 +643,6 @@ int8_t AP_SerialManager::find_portnum(enum SerialProtocol protocol, uint8_t inst return int8_t(_state - &state[0]); } -// should_forward_mavlink_telemetry - returns true if this port should forward telemetry -bool AP_SerialManager::should_forward_mavlink_telemetry(enum SerialProtocol protocol, uint8_t instance) const -{ - const struct UARTState *_state = find_protocol_instance(protocol, instance); - if (_state == nullptr) { - return true; - } - return (_state->options & AP_HAL::UARTDriver::OPTION_MAVLINK_NO_FORWARD) != AP_HAL::UARTDriver::OPTION_MAVLINK_NO_FORWARD; -} - -// get_mavlink_protocol - provides the specific MAVLink protocol for a -// given channel, or SerialProtocol_None if not found -AP_SerialManager::SerialProtocol AP_SerialManager::get_mavlink_protocol(mavlink_channel_t mav_chan) const -{ - uint8_t instance = 0; - uint8_t chan_idx = (uint8_t)(mav_chan - MAVLINK_COMM_0); - for (uint8_t i=0; i -#include #include #ifdef HAL_UART_NUM_SERIAL_PORTS @@ -202,13 +201,6 @@ public: // find_portnum - find port number (SERIALn index) for a protocol and instance, -1 for not found int8_t find_portnum(enum SerialProtocol protocol, uint8_t instance) const; - // should_forward_mavlink_telemetry - returns true if this port should forward telemetry - bool should_forward_mavlink_telemetry(enum SerialProtocol protocol, uint8_t instance) const; - - // get_mavlink_protocol - provides the specific MAVLink protocol for a - // given channel, or SerialProtocol_None if not found - SerialProtocol get_mavlink_protocol(mavlink_channel_t mav_chan) const; - // set_blocking_writes_all - sets block_writes on or off for all serial channels void set_blocking_writes_all(bool blocking); @@ -230,27 +222,46 @@ public: // parameter var table static const struct AP_Param::GroupInfo var_info[]; + class UARTState { + friend class AP_SerialManager; + public: + bool option_enabled(uint16_t option) const { + return (options & option) == option; + } + // returns a baudrate such as 9600. May map from a special + // parameter value like "57" to "57600": + uint32_t baudrate() const { + return AP_SerialManager::map_baudrate(baud); + } + AP_SerialManager::SerialProtocol get_protocol() const { + return AP_SerialManager::SerialProtocol(protocol.get()); + } + private: + AP_Int32 baud; + AP_Int16 options; + AP_Int8 protocol; + }; + + // search through managed serial connections looking for the + // instance-nth UART which is running protocol protocol. + // protocol_match is used to determine equivalence of one protocol + // to another, e.g. MAVLink2 is considered MAVLink1 for finding + // mavlink1 protocol instances. + const UARTState *find_protocol_instance(enum SerialProtocol protocol, + uint8_t instance) const; + private: static AP_SerialManager *_singleton; // array of uart info. See comment above about // SERIALMANAGER_MAX_PORTS - struct UARTState { - AP_Int32 baud; - AP_Int16 options; - AP_Int8 protocol; - } state[SERIALMANAGER_MAX_PORTS]; + UARTState state[SERIALMANAGER_MAX_PORTS]; // pass-through serial support AP_Int8 passthru_port1; AP_Int8 passthru_port2; AP_Int8 passthru_timeout; - // search through managed serial connections looking for the - // instance-nth UART which is running protocol protocol - const UARTState *find_protocol_instance(enum SerialProtocol protocol, - uint8_t instance) const; - // protocol_match - returns true if the protocols match bool protocol_match(enum SerialProtocol protocol1, enum SerialProtocol protocol2) const;