OBDII sending mixed responses

Hello everyone!

I’ve got some problem with getting valid response from OBDII. I’m using ELM327 BT, HC05 in master mode and Arduino Uno/Leonardo.

First of all, when I send single command like 010C for RPM value or 010D for Velocity value and repeat only it there is all fine. But when I try to send command in the row then Arduino receviced mixed responses.

Here is my code:
Main:

#include <Timers.h>
#include <SoftwareSerial.h>
#include <LedControl.h>
//
#define conLed 6            //Arduino pin connected to control Timer Led
#define RxD 9                //Arduino pin connected to Tx of HC-05
#define TxD 8                //Arduino pin connected to Rx of HC-05
#define Reset 7              //Arduino pin connected to Reset of HC-05 (reset with LOW)
#define PIO11 A0             //Arduino pin connected to PI011 of HC-05 (enter AT Mode with HIGH)
//
#define BT_CMD_RETRIES 5     //Number of retries for each Bluetooth AT command in case of not responde with OK
#define OBD_CMD_RETRIES 3    //Number of retries for each OBD command in case of not receive prompt '>' char
#define RPM_CMD_RETRIES 5    //Number of retries for RPM command
#define VELOCITY_CMD_RETRIES 5  //Number of retries for Velocity command
//
int command_index=0;
unsigned int rpm,velocity;   //Variables for RPM
int shift_light_rpm;         //Variable for Shift Light RPM
char bufin_rpm[15];
char bufin_v[12];
boolean bt_error_flag;       //Variable for bluetooth connection error
boolean obd_error_flag;      //Variable for OBD connection error
boolean rpm_error_flag;      //Variable for RPM error
boolean velocity_error_flag;  //Variable for Velocity error
boolean rpm_retries;         //Variable for RPM cmd retries
boolean velocity_retries;    //Variable for Velicty cmd retries
//
LedControl lc=LedControl(12,11,10,1);
SoftwareSerial blueToothSerial(RxD,TxD);
Timers <2> t;
//
void setup(){
 Serial.begin(38400);
  lc.shutdown(0,false);
  lc.setIntensity(0,8);
  lc.clearDisplay(0);
  //
  pinMode(conLed, OUTPUT);
 pinMode(RxD, INPUT);
 pinMode(TxD, OUTPUT);
 pinMode(PIO11, OUTPUT);
 pinMode(Reset, OUTPUT);
  //
 digitalWrite(PIO11, LOW);    //Set HC-05 to Com mode
 digitalWrite(Reset, HIGH);   //HC-05 no Reset
  rpm_retries=0;
  velocity_retries=0;
  //
 setupBlueToothConnection();   //start Bluetooth Connection
 obd_init();                   //OBDII initialitation
  Serial.println("Timer_functions_definiton");// 
  t.attach(0,1000,rpm_calc);        //Define functions depends on timer
  t.attach(1,1000,velo_calc);
  Serial.println("Setup_done");
} 
//
void loop(){
  t.process();  //update of timer for calling rpm_calc
}

rpm_calc

void rpm_calc(){
  boolean prompt_rpm,valid_rpm;  
  char recvChar_rpm;
  //char bufin_rpm[15];
  int i_rpm;
  //
  if (!(obd_error_flag)){                                   //if no OBD connection error
  //
    valid_rpm=false;
    prompt_rpm=false;
    blueToothSerial.print("010C");                        //send to OBD PID command 010C is for RPM, the last 1 is for ELM to wait just for 1 respond (see ELM datasheet)
    blueToothSerial.print("\r");                           //send to OBD cariage return char
    while (blueToothSerial.available() <= 0);              //wait while no data from ELM
    i_rpm=0;
    while ((blueToothSerial.available()>0) && (!prompt_rpm)){  //if there is data from ELM and prompt is false
      recvChar_rpm = blueToothSerial.read();                   //read from ELM
      if ((i_rpm<15)&&(!(recvChar_rpm==32))) {                     //the normal respond to previus command is 010C1/r41 0C ?? ??>, so count 15 chars and ignore char 32 which is space
        bufin_rpm[i_rpm]=recvChar_rpm;                                 //put received char in bufin array
        i_rpm=i_rpm+1;                                             //increase i
      }  
    if (recvChar_rpm==62) prompt_rpm=true;                       //if received char is 62 which is '>' then prompt is true, which means that ELM response is finished 
  }
  }
}
    if ((bufin_rpm[6]=='4') && (bufin_rpm[7]=='1') && (bufin_rpm[8]=='0') && (bufin_rpm[9]=='C')){ //if first four chars after our command is 410C
    valid_rpm=true;                                                                  //then we have a correct RPM response
    } else {
    valid_rpm=false;                                                                 //else we dont
    }
    if (valid_rpm){                                                                    //in case of correct RPM response
      rpm_retries=0;                                                               //reset to 0 retries
      rpm_error_flag=false;                                                        //set rpm error flag to false
            //start calculation of real RPM value
            //RPM is coming from OBD in two 8bit(bytes) hex numbers for example A=0B and B=6C
            //the equation is ((A * 256) + B) / 4, so 0B=11 and 6C=108
            //so rpm=((11 * 256) + 108) / 4 = 731 a normal idle car engine rpm
      rpm=0;                                                                                      
      for (i_rpm=10;i_rpm<14;i_rpm++){                              //in that 4 chars of bufin array which is the RPM value
          if ((bufin_rpm[i_rpm]>='A') && (bufin_rpm[i_rpm]<='F')){        //if char is between 'A' and 'F'
            bufin_rpm[i_rpm]-=55;                                 //'A' is int 65 minus 55 gives 10 which is int value for hex A
          } 
          if ((bufin_rpm[i_rpm]>='0') && (bufin_rpm[i_rpm]<='9')){        //if char is between '0' and '9'
            bufin_rpm[i_rpm]-=48;                                 //'0' is int 48 minus 48 gives 0 same as hex
          }
          rpm=(rpm << 4) | (bufin_rpm[i_rpm] & 0xf);              //shift left rpm 4 bits and add the 4 bits of new char
      }
      rpm=rpm >> 2;                                     //finaly shift right rpm 2 bits, rpm=rpm/4
      Serial.print("Obroty: ");
      Serial.println(rpm);
    }

    //
    if (!valid_rpm){                                              //in case of incorrect RPM response
      rpm_error_flag=true;                                    //set rpm error flag to true
      rpm_retries+=1;                                         //add 1 retry
      rpm=0;                                                  //set rpm to 0
      Serial.print("RPM_ERROR_BUFIN: ");
      Serial.println(bufin_rpm); 
      if (rpm_retries>=RPM_CMD_RETRIES) obd_error_flag=true;  //if retries reached RPM_CMD_RETRIES limit then set obd error flag to true
    }
  }
}

Response if only RPM function is active below:

RPM_BUFIN: 010C1410C110A 11 E7 >ING... 41 0C 11 6D >010C1 41 0C 11 0A >010C141 0CM?
RPM_BUFIN: 010C1410C11E7 11 E7 >010C1 41 0C 14 32 >>010C1 41 0C 11 0A >010C141 0C??

And if both function are active:

RPM_BUFIN: 010C1010D1410410D00 >D1 DD >41 0D 00 >010C1010D141 0C 0C 98 >41 0D 00 > 0C 0CD?
VELOCITY_BUFIN: 410D00 >D1 DD >41 0D 00 >010C1010D141 0C 0C 98 >41 0D 00 > 0C 0CG?
RPM_BUFIN: 010C1010D1410410D00 >D1010D141 0C 0C 41 >41 0D 00 > 0C 0C 98 >41 0D 00 >010C1 |?
VELOCITY_BUFIN: 410D00 >D1010D141 0C 0C 41 >41 0D 00 > 0C 0C 98 >41 0D 00 >010C1?

What could be wrong? Could you give me some idea what I have to change to get proper response?

Post complete code, not excerpts. The problem could be in any hidden part.