     // if programming failed, don't try to do anything
     if (!dmpReady) return;
 
-    if (sensorDataReady)
+    while (sensorDataReady)
         readSensor(az, el);
 
 
     if (once)
     {
         //let it stabilize for 5seconds
-        if (timeElapsed > 2000 + 1000 * (100 / SENSOR_6050_FIFO_HZ ))
+        if (timeElapsed > 10000 + 1000 * (200 / SENSOR_6050_FIFO_HZ ))
         {
             //after pressing "reset" current position will be used as zero
             az0 = az;
             el0 = el;
-            once = false;
-            timeElapsed = 0;
             az.clear(az0);
             el.clear(el0);
+            timeElapsed = 0;
             light = 0;
+            hadUsb = 60000;
+            interrupts();
+            once = false;
         }
     }
     else
     {
+        interrupts();
         while (Serial.available() >= 3 + 9)
         {
-
+            interrupts();
             if (Serial.find(MESSAGE_HDR))
             {
                 //computer makes S/R request, and arduino just responds
                 Serial.readBytes(msg.buffer, 9);
                 {
-                    my_helpers::no_interrupts lock;
                     if (msg.message.Command == 'S') //set
                     {
                         float azm = 0;
                         float elm = 0;
 
                         ard_st::readAzEl(msg.message.value, &azm, &elm);
-
+                        my_helpers::no_interrupts dumb;
                         az0 = az - azm;
                         el0 = el - elm;
-                        az.push_back(azm);
-                        el.push_back(elm);
+                        az.clear(azm);
+                        el.clear(elm);
                         continue;
                     }
-                    if (msg.message.Command == 'R') //read, other part of message must be present but ignored
+                    if (msg.message.Command == 'R' && light > 250) //read, other part of message must be present but ignored
                     {
                         ard_st::packAzEl(msg.message.value, getAz(az - az0), el - el0);
                         Serial.write(msg.message.value.buffer, sizeof(msg.message.value.buffer));

#endif

#define LCD_BACK_LIGHT_SENS_DEGREE (0.15) //sensevity of when to turn on backlight
#define SENSOR_6050_FIFO_HZ 10 //my update to motion_api, should be defined prior include
#define USE_LCD
#define INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
#define READINGS_AMOUNT_AVR static_cast<uint8_t>((SENSOR_6050_FIFO_HZ) / 10 + 3) //how many reading to use to calc avr

    static bool once = true;
    static elapsedMillis timeElapsed;
    static elapsedMillis light;
    static elapsedMillis hadUsb;

    static my_helpers::Circular<decltype(az0) , READINGS_AMOUNT_AVR> az(0);
    static my_helpers::Circular<decltype(el0) , READINGS_AMOUNT_AVR> el(0);
