Can`t receive MSGs from Pixhawk4 to STM32?

Hi, i am new to MAVLink and PX4 so i decided to learn by creating little project.
I have Pixhawk4 and old STM32F411CEU6 microcontroller and i want to capture information with it.
Ofcource i read greate article by Juan Pedro López (MAVLink and Arduino: step by step). It answered to many of my questions, but still it does not work in my case…

PROBLEM:

In the end I want to control custom gimbal but for now it would be nice to just receive some simplest MSGs from Pixhawk4 in STM32F411CEU6 , as it was in the article. BUT i can not figure out why it does not work. It is not clear what exactly the problem is. All i see is that inpSerial always empty whitch means either i have a problem with STM32 or requesting messages.

The good news is that i can see Heartbeat in MissionPlaner.

MY CODE AND HARDWARE:

I write in Arduino IDE (settings for STM32 i fount in some article) and use FTDI arduino to display messages in Termit app .

#include <Arduino.h>
#include <SoftwareSerial.h>                             // http://arduino.cc/en/Reference/softwareSerial
#include <mavlink.h>


#define rxInp PA3
#define txInp PA2
#define rxOut PB7
#define txOut PB6

SoftwareSerial inpSerial(rxInp, txInp);                     //PX -> STM32
SoftwareSerial outSerial(rxOut, txOut);                     //STM32 -> 

//------------------------------ MACROS -----------------------------
#define MAV_TYPE_GIMBAL 26       //temporary here

// Debug:
#define DEBUG_MODE

//---------------------------- VARIABLES ----------------------------
// Mavlink variables:
unsigned long previousMillisMAVLink   = 0;              // will store last time MAVLink was transmitted and listened
unsigned long nextIntervalMAVLink     = 1000;           // next interval to count
const int     c_num_hbs               = 60;             // № of heartbeats to wait before activating STREAMS from Pixhawk. 60 = one minute.
int           num_hbs_pasados         = c_num_hbs;

//Sets the data rate in bits per second
const int     c_connectionSpeed       = 57600;

//MAVLink
int           sysid                   = 1;                       ///< ID 1: id of plane (stm32 is planes` component)
int           compid                  = 158;                     ///< The component sending the message
int           type                    = MAV_TYPE_GIMBAL /*MAV_TYPE_FIXED_WING*/;         ///< Gimbal

// Define the system type, in this case an airplane -> on-board controller
uint8_t       system_type             = MAV_TYPE_GENERIC;
uint8_t       autopilot_type          = MAV_AUTOPILOT_INVALID;

uint8_t       system_mode             = MAV_MODE_PREFLIGHT;      ///< Booting up
uint32_t      custom_mode             = 0;                       ///< Custom mode, can be defined by user/adopter
uint8_t       system_state            = MAV_STATE_STANDBY;       ///< System ready for flight

void setup()
{
  pinMode(rxInp, INPUT);
  pinMode(txInp, OUTPUT);
  pinMode(rxOut, INPUT);
  pinMode(txOut, OUTPUT);
  
  inpSerial.begin(c_connectionSpeed);
  outSerial.begin(c_connectionSpeed);
}

void loop() 
{
  // Initialize the required buffers
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  // Pack the message
  //mavlink_msg_heartbeat_pack(sysid,compid, &msg, type, autopilot_type, system_mode, custom_mode, system_state);
  mavlink_msg_heartbeat_pack(1,0, &msg, type, autopilot_type, system_mode, custom_mode, system_state);  //OLD


  // Copy the message to the send buffer
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

  // Send the message with the standard UART send function
  // uart0_send might be named differently depending on
  // the individual microcontroller / library in use.
  unsigned long currentMillisMAVLink = millis();
  
  if (currentMillisMAVLink - previousMillisMAVLink >= nextIntervalMAVLink) {
    // Save the last time you changed the mode
    previousMillisMAVLink = currentMillisMAVLink;

    inpSerial.write(buf,len);

#ifdef DEBUG_MODE
    outSerial.println(" HB         [-----SENT-----]");
#endif

    num_hbs_pasados++;
    if(num_hbs_pasados>=c_num_hbs) {
      Mav_Request_Data();
      num_hbs_pasados=0;
    }
  }
  comm_receive();
}

//------------------------ Mav_Request_Data()------------------------
//  1 - Request from Pixhawk what DATA SETs STM32 wants;
//  2 - Request transmission rate.
void Mav_Request_Data()
{
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

// To be setup according to the needed information to be requested from the Pixhawk
  const int       maxStreams              = 2;
  const uint8_t   MAVStreams[maxStreams]  = {MAV_DATA_STREAM_ALL/*MAV_DATA_STREAM_EXTENDED_STATUS*//*, MAV_DATA_STREAM_EXTRA1*/};
  const uint16_t  MAVRates[maxStreams]    = {0x02/*, 0x05*/};            //data rate in Hz (times per second)

    
  for (int i=0; i < maxStreams; i++) {
    //Pack
    //v1
    mavlink_msg_request_data_stream_pack(2, 200, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1);    //OLD
//    mavlink_msg_request_data_stream_pack(sysid, compid, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1);

//    //v2
//    const uint16_t MAVStreamID= 0;    //HB
//    const uint32_t MAVRates = 511;
//    mavlink_msg_message_interval_pack(2, 200, &msg, MAVStreamID, MAVRates);
    
    uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
    
    //Send
    inpSerial.write(buf,len);
    
#ifdef DEBUG_MODE
    outSerial.println(" MAVLink    [--REQUESTED!--].");
#endif
  }
}
//-------------------------- comm_receive()--------------------------
// 1 - Focus on what messages we are interested in and how to decode them
void comm_receive() 
{
#ifdef DEBUG_MODE
  //outSerial.println(" comm_receive() [open].");
#endif

  mavlink_message_t msg;
  mavlink_status_t status;

  while(inpSerial.available()>0) {                  // PROBLEM!!!!!!
    
    uint8_t c = inpSerial.read();

#ifdef DEBUG_MODE
      outSerial.println("----while works----");
#endif
    
    // Try to get a new message
    if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {

#ifdef DEBUG_MODE
      outSerial.print("DEBUG msgID: ");
      outSerial.println(msg.msgid);
#endif
      // Handle message
      switch(msg.msgid) 
      {
#ifdef DEBUG_MODE            <TEST>
        case MAVLINK_MSG_ID_HEARTBEAT:  // #0:
          {
            mavlink_heartbeat_t hb;
            mavlink_msg_heartbeat_decode(&msg, &hb);

            outSerial.print("custom_mode:       ");outSerial.println(hb.custom_mode);
            outSerial.print("type:              ");outSerial.println(hb.type);
            outSerial.print("autopilot:         ");outSerial.println(hb.autopilot);
            outSerial.print("base_mode:         ");outSerial.println(hb.base_mode);
            outSerial.print("system_status:     ");outSerial.println(hb.system_status);
            outSerial.print("mavlink_version:   ");outSerial.println(hb.mavlink_version);
          }
          break;
        case MAVLINK_MSG_ID_RC_CHANNELS_RAW:  // #1: SYS_STATUS
          {
            mavlink_rc_channels_raw_t chs;
            mavlink_msg_rc_channels_raw_decode(&msg, &chs);

            outSerial.print("Roll: ");outSerial.println(chs.chan1_raw);
          }
          break;
        case MAVLINK_MSG_ID_ATTITUDE:
          {
            mavlink_attitude_t attitude;
            mavlink_msg_attitude_decode(&msg, &attitude);

            outSerial.print("PX ATTITUDE: ");
            outSerial.println(attitude.roll);
          }
          break;
#endif
        case MAVLINK_MSG_ID_COMMAND_LONG:
        {
          if(outSerial.available()>0)
          { 
            //outSerial.write(c);
#ifdef DEBUG_MODE
            //Tets case
            outSerial.println(" Income MSG [-COMMAND_LONG-].");
#endif
          }
        }
          break;
        default:
#ifdef DEBUG_MODE
          outSerial.println(" Income MSG [--NO_SIMILAR--].");
#endif
          break;
      }
    }
  }
#ifdef DEBUG_MODE
    //outSerial.println(" comm_receive() [close].");
#endif
}

PHOTO: