check i2c_busy in isr too

cubesatsim/cubesatsim.ino

@@ -2591,13 +2591,21 @@ void read_payload()
     if (bmePresent) {
 //    	sprintf(str, "%4.2f %6.2f %6.2f %5.2f ",   
 	while (i2c_busy_now) { Serial.print(".");}
+	i2c_busy_now = true;    
 	reading1 = bme.readTemperature();
+	i2c_busy_now = false;    	    
 	while (i2c_busy_now) { Serial.print(".");}
+	i2c_busy_now = true;    
 	reading2 = bme.readPressure();
+	i2c_busy_now = false;    	    
 	while (i2c_busy_now) { Serial.print(".");}
+	i2c_busy_now = true;    
 	reading3 = bme.readAltitude(SEALEVELPRESSURE_HPA);
+	i2c_busy_now = false;    	    
 	while (i2c_busy_now) { Serial.print(".");}
+	i2c_busy_now = true;    
 	reading4 = bme.readHumidity();		
+	i2c_busy_now = false;    	    
    	sprintf(str, "%.1f %.2f %.1f %.2f ", 
 //	  bme.readTemperature(), bme.readPressure() / 100.0, bme.readAltitude(SEALEVELPRESSURE_HPA), bme.readHumidity());
 	  reading1, reading2 / 100.0, reading3, reading4);
@@ -2609,20 +2617,34 @@ void read_payload()
     if (mpuPresent) 	 { 
 //    print_string(payload_str);
 	while (i2c_busy_now) { Serial.print(".");}	    
+	i2c_busy_now = true;    
       mpu6050.update();
+	i2c_busy_now = false;    	    
 	    
 	while (i2c_busy_now) { Serial.print(".");}
+	i2c_busy_now = true;    
 	reading1 = mpu6050.getGyroX();
+	i2c_busy_now = false;    	    
 	while (i2c_busy_now) { Serial.print(".");}
+	i2c_busy_now = true;    
 	reading2 = mpu6050.getGyroY();
+	i2c_busy_now = false;    	    
 	while (i2c_busy_now) { Serial.print(".");}
+	i2c_busy_now = true;    
 	reading3 = mpu6050.getGyroZ();
+	i2c_busy_now = false;    	    
 	while (i2c_busy_now) { Serial.print(".");}
+	i2c_busy_now = true;    
 	reading4 = mpu6050.getAccX();		    
+	i2c_busy_now = false;    	    
 	while (i2c_busy_now) { Serial.print(".");}
+	i2c_busy_now = true;    
 	reading3 = mpu6050.getAccY();
+	i2c_busy_now = false;    	    
 	while (i2c_busy_now) { Serial.print(".");}
+	i2c_busy_now = true;    
 	reading4 = mpu6050.getAccZ();	
+	i2c_busy_now = false;    	    
 	    
 //    sprintf(str, " MPU6050 %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f ",
       sprintf(str, "MPU6050 %.1f %.1f %.1f %.1f %.1f %.1f ",
@@ -3635,9 +3657,12 @@ bool TimerHandler0(struct repeating_timer *t) {
       digitalWrite(BPSK_CONTROL_A, HIGH);  
 //      Serial.print("-");	    
       if (mode == FSK) {
+        if (!i2c_busy_now) {
 	      i2c_busy_now = true;
-//	      clockgen.enableOutputOnly(1);	  
-   	      i2c_busy_now = false;	      
+/	      clockgen.enableOutputOnly(1);	  
+   	      i2c_busy_now = false;	
+        } else
+		Serial.print("-");
       }
     } else {
 //      digitalWrite(BPSK_CONTROL_A, LOW);  
@@ -3645,11 +3670,14 @@ bool TimerHandler0(struct repeating_timer *t) {
 //      delayMicroseconds(10);    	  
       digitalWrite(BPSK_CONTROL_B, HIGH);	
 //      Serial.print("_");	 
-      if (mode == FSK) {
+       if (mode == FSK) {
+         if (!i2c_busy_now) {
 	      i2c_busy_now = true;
-//	      clockgen.enableOutputOnly(0);	
+	      clockgen.enableOutputOnly(0);	
 	      i2c_busy_now = false;	      
-      }
+         } else
+		Serial.print("-");
+       }
     }	
     if (wav_position > bufLen) { // 300) {
 	wav_position = wav_position % bufLen;