Problem reading data from custom module via serial port

Development Team
1

Good time of day everyone,
i made my AP_MyCustomDriver library based on rangefinde that reads serial port to ardupilot firmware. I use 4.5.6 version of firmware, pixhawk 2.4.8 fc, and my module that sends data by UART.

The problem is that I can’t figure out why is the data being red incorrectly. It supposed to read this message:
SOLVE: x = 0.3 | y = -4.655 | z = 1.0 | xf = 1.215 | yf = -1.15 | zf = 1.0 | sq = 87 | anc_num = 3 | rx_master_timestamp = 0.855660 | valid_data = 1
But for different params of call rate and expected time for SCHED_TASK it gives different variations of the message but never the right one.
For example with this params
SCHED_TASK(read_mycustomdriver, 40, 180, 10),
I got this output

image
image303×409 79.8 KB

image
image310×397 77.9 KB

Here is the code I added:
libraries/AP_MyCustomDriver/AP_MyCustomDriver.h

#pragma once

#include <AP_HAL/AP_HAL.h>
#include <GCS_MAVLink/GCS.h>
#include <cstdio>

class MyCustomDriver {
public:
    MyCustomDriver();
    void init();
    void update();
private:
    AP_HAL::UARTDriver* uart;
    char buffer[256];
    uint8_t buffer_idx = 0;
    void parse_buffer();
};

libraries/AP_MyCustomDriver/AP_MyCustomDriver.cpp

#include "AP_MyCustomDriver.h"

extern const AP_HAL::HAL &hal;
MyCustomDriver::MyCustomDriver() {}

void MyCustomDriver::init() {
    uart = hal.serial(2);
    if (uart != nullptr) {
        uart->set_stop_bits(1);
        uart->begin(115200);
    }
}

void MyCustomDriver::update() {
    if (uart == nullptr) {
        return;
    }
    while(uart->available()) {
        char c = uart->read();
        gcs().send_text(MAV_SEVERITY_INFO, "in %d byte i read %i as %c symbol",buffer_idx,c,c);
        if (c=='\n' || buffer_idx >= sizeof(buffer)-1) {
            if (c == '\n') gcs().send_text(MAV_SEVERITY_INFO, "i read \\n");
            parse_buffer();
            buffer_idx = 0;
            buffer[buffer_idx] = 0;
            break;
        } else {
            buffer[buffer_idx++] = c;
        }
    }
}   

void MyCustomDriver::parse_buffer() {
gcs().send_text(MAV_SEVERITY_INFO, "I read %s", buffer);
}

ArduCopter/Copter.cpp

    SCHED_TASK(read_mycustomdriver,   40, 180,   10),

ArduCopter/Copter.h

    void read_mycustomdriver(void);
    void init_mycustomdriver(void);

ArduCopter/system.cpp

void Copter::init_mycustomdriver(void) {
    mycustomdriver.init();
}

void Copter::read_mycustomdriver(void) {
    mycustomdriver.update();
}

AP_Vehicle/AP_Vehicle.h

#include <AP_MyCustomDriver/AP_MyCustomDriver.h>


    MyCustomDriver mycustomdriver;

Thanks for any help