diff --git a/src/imu/imu.cpp b/src/imu/imu.cpp index 5c4b615..17a95f0 100644 --- a/src/imu/imu.cpp +++ b/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（角度制）===== -YPR dmp() { - YPR out = {0, 0, 0}; - if (!dmpReady) return out; +// ===== DMP 读取，返回单位重力向量在 XYZ 轴上的投影 ===== + +GravityVector dmp() { + 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(packetSize)); + fifoCount -= packetSize; + } - if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); - float ypr[3]; - mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); - - out.yaw = ypr[0] * 180.0f / M_PI; - out.pitch = ypr[1] * 180.0f / M_PI; - out.roll = ypr[2] * 180.0f / M_PI; + lastGravity.x = gravity.x; + lastGravity.y = gravity.y; + lastGravity.z = gravity.z; } - // Serial.print("Yaw: "); Serial.print(a.yaw); - // Serial.print(" Pitch: "); Serial.print(a.pitch); - // Serial.print(" Roll: "); Serial.println(a.roll); - return out; + return lastGravity; } // ===== 原始数据读取，返回 RawData ===== diff --git a/src/imu/imu.h b/src/imu/imu.h index 85d280d..17598e3 100644 --- a/src/imu/imu.h +++ b/src/imu/imu.h @@ -3,11 +3,11 @@ #include -// 返回姿态角（单位：度） -struct YPR { - float yaw; - float pitch; - float roll; +// 单位重力向量在传感器 XYZ 轴上的投影 +struct GravityVector { + float x; + float y; + float z; }; // 返回原始数据及换算值 @@ -20,7 +20,7 @@ struct RawData { }; // 函数声明 -YPR dmp(); +GravityVector dmp(); RawData raw(); void imu_setup(); diff --git a/src/main.cpp b/src/main.cpp index 5c912ed..7c18a86 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -3,6 +3,8 @@ #include "wifi/wifi-sg90.h" #include +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; - unsigned long now = millis(); - //unsigned long now = micros(); // 改用微秒级计时 + static uint32_t lastTelemetry = 0; + uint32_t 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); } } \ No newline at end of file diff --git a/src/wifi/wifi-sg90.h b/src/wifi/wifi-sg90.h index 0b3aa51..1798abe 100644 --- a/src/wifi/wifi-sg90.h +++ b/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 -// values (yaw/pitch/roll in centidegrees, then ax/ay/az/gx/gy/gz), then -// CRC-16/CCITT-FALSE over the first 20 bytes. Text command replies remain lines. -void sendLineData(YPR a, RawData r); +// TCP IMU telemetry: 16-byte frame: A5 5A, then 6 signed big-endian int16 +// values (projected gravity XYZ scaled by 10000, base angular velocity XYZ +// in mrad/s), then CRC-16/CCITT-FALSE over the first 14 bytes. +// Text command replies remain lines. +void sendTelemetry(GravityVector gravity, RawData raw); #endif diff --git a/src/wifi/wifi.cpp b/src/wifi/wifi.cpp index 32d2ba5..2efcf4f 100644 --- a/src/wifi/wifi.cpp +++ b/src/wifi/wifi.cpp @@ -1,6 +1,8 @@ #include #include #include +#include +#include #include #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) 处触发 - g_servos[i].lastCycle = micros() - - (unsigned long)i * (PERIOD_US / NUM_SERVOS); + ledcSetup(i, 50, SERVO_PWM_RESOLUTION); + ledcAttachPin(SERVO_PINS[i], i); - pinMode(SERVO_PINS[i], OUTPUT); - digitalWrite(SERVO_PINS[i], LOW); + const uint32_t pulseUs = + 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; + float rad = s.targetRad; + if (rad < RAD_MIN) rad = RAD_MIN; + if (rad > RAD_MAX) rad = RAD_MAX; - // 若长时间滞后（例如遇到耗时长的指令），重新对齐到当前时刻 - 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 + - (uint16_t)((PULSE_MAX_US - PULSE_MIN_US) * - (rad / PI) + 0.5f); - - digitalWrite(s.pin, HIGH); - delayMicroseconds(pulse); - digitalWrite(s.pin, LOW); + const uint32_t pulseUs = PULSE_MIN_US + + static_cast((PULSE_MAX_US - PULSE_MIN_US) * + (rad / PI) + 0.5f); + const uint32_t duty = (pulseUs * SERVO_PWM_MAX_DUTY) / 20000U; + if (duty != s.duty) { + s.duty = duty; + 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; } - TelemetryData data; - if (telemetryQueue == nullptr || - xQueueReceive(telemetryQueue, &data, 0) != pdTRUE) { - return; + if (!pendingPacketReady) { + TelemetryData data; + if (telemetryQueue == nullptr || + xQueueReceive(telemetryQueue, &data, 0) != pdTRUE) { + return; + } + + // Frame: gravity XYZ, base angular velocity XYZ, then CRC-16/CCITT-FALSE. + pendingPacket[0] = 0xA5; + pendingPacket[1] = 0x5A; + writeInt16BE(pendingPacket, 2, encodeGravityProjection(data.gravity.x)); + writeInt16BE(pendingPacket, 4, encodeGravityProjection(data.gravity.y)); + writeInt16BE(pendingPacket, 6, encodeGravityProjection(data.gravity.z)); + writeInt16BE(pendingPacket, 8, encodeAngularVelocity(data.raw.gx)); + writeInt16BE(pendingPacket, 10, encodeAngularVelocity(data.raw.gy)); + writeInt16BE(pendingPacket, 12, encodeAngularVelocity(data.raw.gz)); + uint16_t crc = calculateCrc16(pendingPacket, 14); + pendingPacket[14] = static_cast(crc >> 8); + pendingPacket[15] = static_cast(crc); + pendingPacketOffset = 0; + pendingPacketReady = true; } - // Frame: A5 5A, 3 angles (centidegrees), 6 raw int16 values, CRC16-CCITT. - uint8_t packet[22] = {0xA5, 0x5A}; - writeInt16BE(packet, 2, encodeAngle(data.attitude.yaw)); - writeInt16BE(packet, 4, encodeAngle(data.attitude.pitch)); - writeInt16BE(packet, 6, encodeAngle(data.attitude.roll)); - writeInt16BE(packet, 8, data.raw.ax); - writeInt16BE(packet, 10, data.raw.ay); - writeInt16BE(packet, 12, data.raw.az); - writeInt16BE(packet, 14, data.raw.gx); - writeInt16BE(packet, 16, data.raw.gy); - writeInt16BE(packet, 18, data.raw.gz); - uint16_t crc = calculateCrc16(packet, 20); - packet[20] = static_cast(crc >> 8); - packet[21] = static_cast(crc); - - if (client.write(packet, sizeof(packet)) != sizeof(packet)) { - Serial.println("警告：IMU 二进制数据发送不完整"); + const int sent = send( + client.fd(), + pendingPacket + pendingPacketOffset, + sizeof(pendingPacket) - pendingPacketOffset, + MSG_DONTWAIT); + if (sent > 0) { + pendingPacketOffset += static_cast(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) { - long scaled = lroundf(degrees * 100.0f); +int16_t encodeAngularVelocity(int16_t gyroRaw) { + 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(gyroRaw) / GYRO_COUNTS_PER_DPS) * + RAD_PER_DEG * 1000.0f); + if (scaled > 32767L) scaled = 32767L; + if (scaled < -32768L) scaled = -32768L; + return static_cast(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(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); }