diff --git a/arduino/Payload_BME280_MPU6050_STM32.ino b/arduino/Payload_BME280_MPU6050_STM32.ino
index 56bddc74..45d9783a 100644
--- a/arduino/Payload_BME280_MPU6050_STM32.ino
+++ b/arduino/Payload_BME280_MPU6050_STM32.ino
@@ -12,6 +12,7 @@ MPU6050 mpu6050(Wire);
 
 int counter = 0;
 long timer = 0;
+int bmePresent;
 
 void setup() {
 
@@ -25,10 +26,12 @@ void setup() {
     digitalWrite(PC13, LOW);   // turn the LED on 
     delay(50);              // wait for a second
     digitalWrite(PC13, HIGH);    // turn the LED off
- 
-  if (!bme.begin(0x76)) {
+
+  if (bme.begin(0x76)) {
+    bmePresent = 1;
+  } else {
     Serial.println("Could not find a valid BME280 sensor, check wiring!");
-    while (1);
+    bmePresent = 0;
   }
   
   mpu6050.begin();
@@ -39,22 +42,30 @@ void loop() {
 
 #ifdef TESTING
   if (Serial.available() > 0) {
-    
     digitalWrite(PC13, LOW);   // turn the LED on
     delay(50);              // wait for a second
     digitalWrite(PC13, HIGH);    // turn the LED off
-
     char result = Serial.read();
-    //    Serial1.println(result);
-    Serial.print("OK BME280 ");
-    Serial.print(bme.readTemperature());
-    Serial.print(" ");
-    Serial.print(bme.readPressure() / 100.0F);
-    Serial.print(" ");
-    Serial.print(bme.readAltitude(SEALEVELPRESSURE_HPA));
-    Serial.print(" ");
-    Serial.print(bme.readHumidity());
-    
+ //       Serial.println(result);  
+     
+    if (result == 'R') {
+      Serial.println("OK");
+      delay(500);
+      setup(); 
+    }
+    if (bmePresent) {
+      Serial.print("OK BME280 ");
+      Serial.print(bme.readTemperature());
+      Serial.print(" ");
+      Serial.print(bme.readPressure() / 100.0F);
+      Serial.print(" ");
+      Serial.print(bme.readAltitude(SEALEVELPRESSURE_HPA));
+      Serial.print(" ");
+      Serial.print(bme.readHumidity());
+    } else
+    {
+       Serial.print("OK BME280 0.0 0.0 0.0 0.0");
+    }
     mpu6050.update();
     
     Serial.print(" MPU6050 ");
@@ -68,24 +79,30 @@ void loop() {
   }
 #else
   if (Serial1.available() > 0) {
-
     digitalWrite(PC13, LOW);   // turn the LED on
     delay(50);              // wait for a second
     digitalWrite(PC13, HIGH);    // turn the LED off
-
     char result = Serial1.read();
 //    Serial1.println(result);
-//    Serial1.println("OK ");
-  
-    Serial1.print("OK BME280 ");
-    Serial1.print(bme.readTemperature());
-    Serial1.print(" ");
-    Serial1.print(bme.readPressure() / 100.0F);
-    Serial1.print(" ");
-    Serial1.print(bme.readAltitude(SEALEVELPRESSURE_HPA));
-    Serial1.print(" ");
-    Serial1.print(bme.readHumidity());
 
+    if (result == 'R') {
+      Serial1.println("OK");
+      delay(500);
+      setup(); 
+    }  
+    if (bmePresent) {  
+      Serial1.print("OK BME280 ");
+      Serial1.print(bme.readTemperature());
+      Serial1.print(" ");
+      Serial1.print(bme.readPressure() / 100.0F);
+      Serial1.print(" ");
+      Serial1.print(bme.readAltitude(SEALEVELPRESSURE_HPA));
+      Serial1.print(" ");
+      Serial1.print(bme.readHumidity());
+    } else
+    {
+       Serial1.print("OK BME280 0.0 0.0 0.0 0.0");
+    }
     mpu6050.update();
     
     Serial1.print(" MPU6050 ");
@@ -98,6 +115,6 @@ void loop() {
 //  Serial1.println(counter++);
   }
 #endif
-
+  
   delay(100);
 }