 
 void loop()
 {
-  // noInterrupts();
-
-  //Update servo values
-  for (int i=0;i<SERVO_NUM;i++)
-    {
-      MOTOR[i].writeMicroseconds(rcv_data[i].i);
-    }
-
-  //  interrupts();
+  //nothing to do here
+}
+
+//on Receive
+void SetServos(int byteCount)
+{
+
+// the RPI shoud send 4 x 1 uint16_t (2 bytes) values
+// one for each PID updated Motor speed
+
+  digitalWrite(LED_PIN,HIGH);
+  if (byteCount == 8)
+    {
+
+      //Serial.println(Wire.available());
+
+      while(Wire.available()) {
+
+	for (int i=0;i<SERVO_NUM;i++)
+	  {
+	    rcv_data[i].b[0] = Wire.read(); //upper bits?
+	    rcv_data[i].b[1] = Wire.read(); //lower bits?
+	  }
+      }
+      //update servos
+      for (int i=0;i<SERVO_NUM;i++)
+	{
+	  MOTOR[i].writeMicroseconds(rcv_data[i].i);
+	}
+
+    }
+
+  digitalWrite(LED_PIN,LOW);
+
+  return;
 }
 
 

      bUpdateFlagsShared |= ROLL_FLAG;
    }
}


void SetServos(int byteCount)
{

// the RPI shoud send 4 x 1 uint16_t (2 bytes) values
// one for each PID updated Motor speed
  if (byteCount == 8)
    {
      while(Wire.available()) {

	for (int i=0;i<SERVO_NUM;i++)
	  {
	    rcv_data[i].b[0] = Wire.read(); //upper bits?
	    rcv_data[i].b[1] = Wire.read(); //upper bits?

	  }
      }
    }

  return;
}


  union Sharedblock
  {
    byte b[2]; // utiliser char parts[4] pour port série
    uint16_t ui;
  } RCsignal[4];


  //Note we don't use shared volatile vars
  //sot that ISRs keep priority on them
  RCsignal[0].ui= unThrottleInShared;
  RCsignal[1].ui= unYawInShared;
  RCsignal[2].ui= unPitchInShared;
  RCsignal[3].ui= unRollInShared;

  byte buffer[8];
  for (int i=0;i<4;i++)
    {
    for (int ii=0;ii<2;ii++)
      {
	buffer[ii+2*i] = RCsignal[i].b[ii];
      }
    }

  Wire.write(buffer,8);

}

