ping < 20ms

src/imu/imu.cpp

@@ -10,11 +10,14 @@ uint8_t fifoBuffer[64];
 
 Quaternion q;
 VectorFloat gravity;
+GravityVector lastGravity = {0, 0, 0};
 
 // ===== 初始化 =====
 void imu_setup() {
   Serial.begin(115200);
   Wire.begin(21, 22);
+  Wire.setClock(400000);
+  Wire.setTimeOut(5);
 
   mpu.initialize();
   if (!mpu.testConnection()) {
@@ -35,25 +38,40 @@ void imu_setup() {
   }
 }
 
-// ===== DMP 读取，返回 YPR（角度制）=====
+// ===== DMP 读取，返回单位重力向量在 XYZ 轴上的投影 =====
-YPR dmp() {
+
-  YPR out = {0, 0, 0};
+GravityVector dmp() {
-  if (!dmpReady) return out;
+  if (!dmpReady) return lastGravity;
+
+  const uint16_t packetSize = mpu.dmpGetFIFOPacketSize();
+  if (packetSize == 0 || packetSize > sizeof(fifoBuffer)) {
+    return lastGravity;
+  }
+
+  uint16_t fifoCount = mpu.getFIFOCount();
+  if (fifoCount >= 1024) {
+    mpu.resetFIFO();
+    return lastGravity;
+  }
+
+  if (fifoCount >= packetSize) {
+    if (fifoCount > packetSize * 5U) {
+      mpu.resetFIFO();
+      return lastGravity;
+    }
+
+    while (fifoCount >= packetSize) {
+      mpu.getFIFOBytes(fifoBuffer, static_cast<uint8_t>(packetSize));
+      fifoCount -= packetSize;
+    }
 
-  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
     mpu.dmpGetQuaternion(&q, fifoBuffer);
     mpu.dmpGetGravity(&gravity, &q);
-    float ypr[3];
+    lastGravity.x = gravity.x;
-    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+    lastGravity.y = gravity.y;
-
+    lastGravity.z = gravity.z;
-    out.yaw   = ypr[0] * 180.0f / M_PI;
-    out.pitch = ypr[1] * 180.0f / M_PI;
-    out.roll  = ypr[2] * 180.0f / M_PI;
   }
-  // Serial.print("Yaw: ");   Serial.print(a.yaw);
+  return lastGravity;
-  // Serial.print(" Pitch: "); Serial.print(a.pitch);
-  // Serial.print(" Roll: ");  Serial.println(a.roll);
-  return out;
 }
 
 // ===== 原始数据读取，返回 RawData =====

src/imu/imu.h

@@ -3,11 +3,11 @@
 
 #include <Arduino.h>
 
-// 返回姿态角（单位：度）
+// 单位重力向量在传感器 XYZ 轴上的投影
-struct YPR {
+struct GravityVector {
-  float yaw;
+  float x;
-  float pitch;
+  float y;
-  float roll;
+  float z;
 };
 
 // 返回原始数据及换算值
@@ -20,7 +20,7 @@ struct RawData {
 };
 
 // 函数声明
-YPR dmp();
+GravityVector dmp();
 RawData raw();
 void imu_setup();
 

src/main.cpp

@@ -3,6 +3,8 @@
 #include "wifi/wifi-sg90.h"
 #include <esp32-hal.h>
 
+constexpr uint32_t TELEMETRY_INTERVAL_US = 20000;
+
 void setup() {
     // 串口初始化等公共配置
     Serial.begin(115200);
@@ -12,14 +14,13 @@ void setup() {
     wifi_setup();
 }
 void loop() {
-  static unsigned long lastTelemetry = 0;
+  static uint32_t lastTelemetry = 0;
-  unsigned long now = millis();
+  uint32_t now = micros();
-  //unsigned long now = micros(); // 改用微秒级计时
 
-  if (now - lastTelemetry >= 300) {
+  if (now - lastTelemetry >= TELEMETRY_INTERVAL_US) {
     lastTelemetry = now;
-    YPR a = dmp();
+    GravityVector gravity = dmp();
     RawData r = raw();
-    sendLineData(a, r);
+    sendTelemetry(gravity, r);
   }
 }

src/wifi/wifi-sg90.h

@@ -7,9 +7,10 @@
 void sg90();
 void wifi_setup();
 
-// TCP IMU telemetry: 22-byte frame: A5 5A, then 9 signed big-endian int16
+// TCP IMU telemetry: 16-byte frame: A5 5A, then 6 signed big-endian int16
-// values (yaw/pitch/roll in centidegrees, then ax/ay/az/gx/gy/gz), then
+// values (projected gravity XYZ scaled by 10000, base angular velocity XYZ
-// CRC-16/CCITT-FALSE over the first 20 bytes. Text command replies remain lines.
+// in mrad/s), then CRC-16/CCITT-FALSE over the first 14 bytes.
-void sendLineData(YPR a, RawData r);
+// Text command replies remain lines.
+void sendTelemetry(GravityVector gravity, RawData raw);
 
 #endif

src/wifi/wifi.cpp

@@ -1,6 +1,8 @@
 #include <WiFi.h>
 #include <freertos/FreeRTOS.h>
 #include <freertos/queue.h>
+#include <errno.h>
+#include <lwip/sockets.h>
 #include <math.h>
 #include "wifi/wifi-sg90.h"
 
@@ -16,7 +18,8 @@ const uint8_t SERVO_PINS[NUM_SERVOS] = {13, 14, 25, 26, 27, 32, 33, 4};
 
 const uint16_t PULSE_MIN_US = 500;    // 0 rad
 const uint16_t PULSE_MAX_US = 2400;   // π rad
-const uint16_t PERIOD_US    = 20000;  // 50Hz = 20ms
+const uint8_t  SERVO_PWM_RESOLUTION = 16;
+const uint32_t SERVO_PWM_MAX_DUTY = (1UL << SERVO_PWM_RESOLUTION) - 1;
 const float    RAD_MIN      = 0.0f;
 const float    RAD_MAX      = PI;
 
@@ -24,7 +27,7 @@ const float    RAD_MAX      = PI;
 struct ServoState {
     uint8_t       pin;
     float         targetRad;
-    unsigned long lastCycle;
+    uint32_t      duty;
 };
 
 ServoState g_servos[NUM_SERVOS];
@@ -33,11 +36,14 @@ WiFiServer tcpServer(TCP_PORT);
 WiFiClient client;
 
 struct TelemetryData {
-    YPR attitude;
+    GravityVector gravity;
     RawData raw;
 };
 
 QueueHandle_t telemetryQueue = nullptr;
+uint8_t pendingPacket[16];
+size_t pendingPacketOffset = 0;
+bool pendingPacketReady = false;
 char commandBuffer[128];
 size_t commandLength = 0;
 bool commandOverflow = false;
@@ -48,7 +54,8 @@ uint8_t parseFloatList(const String& s, float* out, uint8_t maxCount);
 void    sendLine(const String& msg);
 void    sg90Task(void* parameter);
 void    sendQueuedTelemetry();
-int16_t encodeAngle(float degrees);
+int16_t encodeAngularVelocity(int16_t gyroRaw);
+int16_t encodeGravityProjection(float projection);
 void    writeInt16BE(uint8_t* buffer, size_t offset, int16_t value);
 uint16_t calculateCrc16(const uint8_t* data, size_t length);
 
@@ -58,12 +65,13 @@ void servoInit() {
     for (uint8_t i = 0; i < NUM_SERVOS; i++) {
         g_servos[i].pin       = SERVO_PINS[i];
         g_servos[i].targetRad = PI / 2.0f;   // 默认 90°
-        // 相位错开：第 i 个舵机在 20ms 周期内的 (i * 2.5ms) 处触发
+        ledcSetup(i, 50, SERVO_PWM_RESOLUTION);
-        g_servos[i].lastCycle = micros() -
+        ledcAttachPin(SERVO_PINS[i], i);
-                                (unsigned long)i * (PERIOD_US / NUM_SERVOS);
 
-        pinMode(SERVO_PINS[i], OUTPUT);
+        const uint32_t pulseUs =
-        digitalWrite(SERVO_PINS[i], LOW);
+            PULSE_MIN_US + (PULSE_MAX_US - PULSE_MIN_US) / 2;
+        g_servos[i].duty = (pulseUs * SERVO_PWM_MAX_DUTY) / 20000U;
+        ledcWrite(i, g_servos[i].duty);
     }
 }
 
@@ -71,30 +79,20 @@ void servoInit() {
  * @brief 更新 8 个舵机的 PWM，由 SG90 通讯任务周期调用
  */
 void servoUpdateAll() {
-    unsigned long now = micros();
     for (uint8_t i = 0; i < NUM_SERVOS; i++) {
         ServoState& s = g_servos[i];
 
-        if ((unsigned long)(now - s.lastCycle) >= PERIOD_US) {
-            s.lastCycle += PERIOD_US;
-
-            // 若长时间滞后（例如遇到耗时长的指令），重新对齐到当前时刻
-            if ((unsigned long)(now - s.lastCycle) >= PERIOD_US) {
-                s.lastCycle = now;
-            }
-
-            // 弧度 -> 脉宽（µs）
         float rad = s.targetRad;
         if (rad < RAD_MIN) rad = RAD_MIN;
         if (rad > RAD_MAX) rad = RAD_MAX;
 
-            uint16_t pulse = PULSE_MIN_US +
+        const uint32_t pulseUs = PULSE_MIN_US +
-                             (uint16_t)((PULSE_MAX_US - PULSE_MIN_US) *
+            static_cast<uint32_t>((PULSE_MAX_US - PULSE_MIN_US) *
                                   (rad / PI) + 0.5f);
-
+        const uint32_t duty = (pulseUs * SERVO_PWM_MAX_DUTY) / 20000U;
-            digitalWrite(s.pin, HIGH);
+        if (duty != s.duty) {
-            delayMicroseconds(pulse);
+            s.duty = duty;
-            digitalWrite(s.pin, LOW);
+            ledcWrite(i, duty);
         }
     }
 }
@@ -246,7 +244,7 @@ void processCommand(const String& raw) {
     // 简单日志（不要频繁打印，避免影响时序）
     Serial.printf("SET %u servo(s)\n", n);
 
-    sendLine("ESP32: OK 设置 " + String(n) + " 个舵机");
+    //sendLine("ESP32: OK 设置 " + String(n) + " 个舵机");
 }
 
 void handleClientCommand() {
@@ -279,37 +277,67 @@ void handleClientCommand() {
 
 void sendQueuedTelemetry() {
     if (!client || !client.connected()) {
+        pendingPacketReady = false;
+        pendingPacketOffset = 0;
         return;
     }
 
+    if (!pendingPacketReady) {
         TelemetryData data;
         if (telemetryQueue == nullptr ||
             xQueueReceive(telemetryQueue, &data, 0) != pdTRUE) {
             return;
         }
 
-    // Frame: A5 5A, 3 angles (centidegrees), 6 raw int16 values, CRC16-CCITT.
+        // Frame: gravity XYZ, base angular velocity XYZ, then CRC-16/CCITT-FALSE.
-    uint8_t packet[22] = {0xA5, 0x5A};
+        pendingPacket[0] = 0xA5;
-    writeInt16BE(packet, 2, encodeAngle(data.attitude.yaw));
+        pendingPacket[1] = 0x5A;
-    writeInt16BE(packet, 4, encodeAngle(data.attitude.pitch));
+        writeInt16BE(pendingPacket, 2, encodeGravityProjection(data.gravity.x));
-    writeInt16BE(packet, 6, encodeAngle(data.attitude.roll));
+        writeInt16BE(pendingPacket, 4, encodeGravityProjection(data.gravity.y));
-    writeInt16BE(packet, 8, data.raw.ax);
+        writeInt16BE(pendingPacket, 6, encodeGravityProjection(data.gravity.z));
-    writeInt16BE(packet, 10, data.raw.ay);
+        writeInt16BE(pendingPacket, 8, encodeAngularVelocity(data.raw.gx));
-    writeInt16BE(packet, 12, data.raw.az);
+        writeInt16BE(pendingPacket, 10, encodeAngularVelocity(data.raw.gy));
-    writeInt16BE(packet, 14, data.raw.gx);
+        writeInt16BE(pendingPacket, 12, encodeAngularVelocity(data.raw.gz));
-    writeInt16BE(packet, 16, data.raw.gy);
+        uint16_t crc = calculateCrc16(pendingPacket, 14);
-    writeInt16BE(packet, 18, data.raw.gz);
+        pendingPacket[14] = static_cast<uint8_t>(crc >> 8);
-    uint16_t crc = calculateCrc16(packet, 20);
+        pendingPacket[15] = static_cast<uint8_t>(crc);
-    packet[20] = static_cast<uint8_t>(crc >> 8);
+        pendingPacketOffset = 0;
-    packet[21] = static_cast<uint8_t>(crc);
+        pendingPacketReady = true;
+    }
 
-    if (client.write(packet, sizeof(packet)) != sizeof(packet)) {
+    const int sent = send(
-        Serial.println("警告：IMU 二进制数据发送不完整");
+        client.fd(),
+        pendingPacket + pendingPacketOffset,
+        sizeof(pendingPacket) - pendingPacketOffset,
+        MSG_DONTWAIT);
+    if (sent > 0) {
+        pendingPacketOffset += static_cast<size_t>(sent);
+        if (pendingPacketOffset == sizeof(pendingPacket)) {
+            pendingPacketReady = false;
+            pendingPacketOffset = 0;
+        }
+    } else if (sent < 0 && errno != EAGAIN && errno != EWOULDBLOCK &&
+               errno != EINTR) {
+        Serial.printf("遥测 TCP 发送失败，errno=%d\n", errno);
+        client.stop();
+        pendingPacketReady = false;
+        pendingPacketOffset = 0;
     }
 }
 
-int16_t encodeAngle(float degrees) {
+int16_t encodeAngularVelocity(int16_t gyroRaw) {
-    long scaled = lroundf(degrees * 100.0f);
+    constexpr float GYRO_COUNTS_PER_DPS = 16.4f;  // DMP configures ±2000°/s.
+    constexpr float RAD_PER_DEG = PI / 180.0f;
+    long scaled = lroundf(
+        (static_cast<float>(gyroRaw) / GYRO_COUNTS_PER_DPS) *
+        RAD_PER_DEG * 1000.0f);
+    if (scaled > 32767L) scaled = 32767L;
+    if (scaled < -32768L) scaled = -32768L;
+    return static_cast<int16_t>(scaled);
+}
+
+int16_t encodeGravityProjection(float projection) {
+    long scaled = lroundf(projection * 10000.0f);
     if (scaled > 32767L) scaled = 32767L;
     if (scaled < -32768L) scaled = -32768L;
     return static_cast<int16_t>(scaled);
@@ -342,12 +370,12 @@ void sg90Task(void* parameter) {
         vTaskDelay(1);
     }
 }
-void sendLineData(YPR a, RawData r) {
+void sendTelemetry(GravityVector gravity, RawData raw) {
     if (telemetryQueue == nullptr) {
         return;
     }
 
-    TelemetryData data = {a, r};
+    TelemetryData data = {gravity, raw};
     xQueueOverwrite(telemetryQueue, &data);
 }