BNO055与ESP32C3四元数定位丢失问题求助

使用BNO055传感器搭配ESP32C3开发板，将传感器输出的四元数用于Unity姿态检测时，偶尔出现定位丢失（四元数断层）的情况。已排除MQTT连接影响，断开MQTT后问题仍存在。附上使用的代码及四元数断层截图，恳请帮忙解决。

#include <WiFi.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <PubSubClient.h>
#include <utility/imumaths.h>

// WiFi credentials
const char* ssid = "wifi";
const char* password = "password";
// MQTT broker
const char* mqtt_server = "";
const uint16_t mqtt_port = 1883;

WiFiClient espClient;
PubSubClient client(espClient);

// Sensors
Adafruit_BNO055 bno1 = Adafruit_BNO055(55, 0x28);
Adafruit_BNO055 bno2 = Adafruit_BNO055(56, 0x29);

void setup_wifi() {
  delay(10);
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);

  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
}

void reconnect() {
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    if (client.connect("ESP32_IMU_Client")) {
      Serial.println("connected");
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 2 seconds");
      delay(2000);
    }
  }
}

void setup() {
  Serial.begin(115200);
  setup_wifi();
  client.setServer(mqtt_server, mqtt_port);

  if (!bno1.begin()) {
    Serial.println("BNO055 0x28 not detected");
    while (1);
  }
  if (!bno2.begin()) {
    Serial.println("BNO055 0x29 not detected");
    while (1);
  }

  bno1.setExtCrystalUse(true);
  bno2.setExtCrystalUse(true);
}

void loop() {
  if (!client.connected()) {
    reconnect();
  }
  client.loop();

  imu::Quaternion q2 = bno1.getQuat();
  imu::Quaternion q1 = bno2.getQuat();

  String forearm_msg = String(q1.x(), 4) + "," + String(q1.y(), 4) + "," + String(q1.z(), 4) + "," + String(q1.w(), 4);
  String arm_msg = String(q2.x(), 4) + "," + String(q2.y(), 4) + "," + String(q2.z(), 4) + "," + String(q2.w(), 4);

  client.publish("imu/forearm", forearm_msg.c_str());
  //Serial.println("Forearm: " + forearm_msg);
  client.publish("imu/arm", arm_msg.c_str());
  //Serial.println("Arm: " + arm_msg);
}

内容的提问来源于stack exchange，提问作者Gustavo