zhenai
/
TwoFootBot-arduion
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

ping < 20ms

This commit is contained in:
zhenai 2026-10-19 15:23:15 +08:00
parent f596912057
commit 1183a048b3
5 changed files with 134 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
src/imu/imu.cpp
View File

@ -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<uint8_t>(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 =====

12
src/imu/imu.h
View File

@ -3,11 +3,11 @@
#include <Arduino.h>
// 返回姿态角（单位：度）
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();

13
src/main.cpp
View File

@ -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;
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);
}
}

9
src/wifi/wifi-sg90.h
View File

@ -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

120
src/wifi/wifi.cpp
View File

@ -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) 处触发
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;
// 若长时间滞后（例如遇到耗时长的指令），重新对齐到当前时刻
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) *
const uint32_t pulseUs = PULSE_MIN_US +
static_cast<uint32_t>((PULSE_MAX_US - PULSE_MIN_US) *
(rad / PI) + 0.5f);
digitalWrite(s.pin, HIGH);
delayMicroseconds(pulse);
digitalWrite(s.pin, LOW);
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;
}
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.
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<uint8_t>(crc >> 8);
packet[21] = static_cast<uint8_t>(crc);
// 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<uint8_t>(crc >> 8);
pendingPacket[15] = static_cast<uint8_t>(crc);
pendingPacketOffset = 0;
pendingPacketReady = true;
}
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<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) {
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<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);
}