Дык что не отрабатывает? Не выключает client.publish("adam/head", (uint8_t*)&q, sizeof(q));?
И что там такое накручено с fifoCount ?
Он у вас обновляется только если fifoCount < packetSize, а если от станет равным fifoCount > packetSize, то вызовется mpu.resetFIFO(), но значение fifoCount не...