[DEV] Pixhawk freezes after writing to UART


I am writing protocol, which uses UART as a communication channel. I have done almost everything I wanted to, but one thing remained unsolved.

I looked into FrSky Telem Protocol for inspiration, where I found implementation of following feature:
User chooses protocol for each UART (Serial) in GCS (Mission Planner, APM Planner etc.) and while the autopilot is initialized, it will automatically find, which port is selected for my protocol. Here is the piece of code, it will make it more clear I hope:

if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_D, 0))) {
    _protocol = AP_SerialManager::SerialProtocol_FrSky_D; 

So I think this should set “_port” variable (pointer) to the UART, which was selected by user to use the FrSky_D protocol.

I did exactly the same. I added also dependencies for my protocol (in SerialManager etc etc), I wont be putting it here, if someone is willing to help, we can discuss those later (it would be confusing now imho). Build is totally fine, everything good. I have tested all my functionality with UART hardcoded to Serial5. So with UART hardcoded to Serial5 (hal.uartE), everything is working good.

So to the behavior. When I build the code, it is fine, but after initialization of Pixhawk (immediately after flashing red/blue alternatively), the main LED will remained on (red or blue - it is random). Also the B/E Led is flashing (amber color) which I know is some memory allocation problem.

I will not be explaining further, as I have said, if someone is willing to help, we can discuss it here.

Thank you all for help!