When I try to read data separately it works, but together it stuck every second (probably reading of gps data) and MPU6050 serial monitor sending FIFO overflow error .
Yes, you have to be careful about the loop structure and "blocking" to "wait" for a new GPS speed report. SoftwareSerial also blocks everything else during each received character time.
Just check for GPS characters and update the speed if a complete sentence was received, then do "other things" (e.g., use the IMU data). Here is a NeoGPS version of your example:
#include "NMEAGPS.h"
#include <NeoSWSerial.h>
static const int RXPin = 7, TXPin = 4;
static const uint32_t GPSBaud = 9600;
float gps_speed = 0;
NMEAGPS gps;
// The serial connection to the GPS device
NeoSWSerial ss(RXPin, TXPin);
void setup(){
Serial.begin( 9600 );
ss.begin(GPSBaud);
}
void loop(){
// Read GPS characters and see if a new fix is available
if (gps.available( ss )) {
gps_fix fix = gps.read();
// A fix is available once per second, but the GPS may not know
// the speed value, perhaps because the satellite reception is
// poor or the unit is just starting. Check the valid flag.
if (fix.valid.speed) {
// Update the gps_speed. Other parts of your
// program can use it whenever they want.
gps_speed = fix.speed_kph();
Serial.println( gps_speed );
}
}
}
NeoGPS is smaller and faster than other libraries and can be configured to only parse the fields you really use.
Your original sketch uses 6884 bytes program space and 349 bytes of RAM.
The NeoGPS version can be as small as 4802 bytes program space and 150 bytes of RAM. Significant savings!
For the software serial library, I would suggest using AltSoftSerial on pins 8 & 9, if you can move the GPS. It is much more efficient, and does not disable interrupts for long periods of time: SoftwareSerial will block for ~1ms, and eternity for a 16MHz Arduino. This is probably interfering with the IMU interrupts.
If you can't move the GPS to pins 8 & 9, you might try another library I maintain, NeoSWSerial. It is much more efficient than SoftwareSerial, and it works on any two pins. NeoSWSerial will "play nicer" with the IMU interrupt code, but it can still be affected if the IMU interrupt code takes too long. NeoSWSerial should not affect the IMU interrupt code. I used it in the above sketch, along with a debug print.