first commit
This commit is contained in:
commit
f596912057
|
|
@ -0,0 +1,5 @@
|
|||
.pio
|
||||
.vscode/.browse.c_cpp.db*
|
||||
.vscode/c_cpp_properties.json
|
||||
.vscode/launch.json
|
||||
.vscode/ipch
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
{
|
||||
// See http://go.microsoft.com/fwlink/?LinkId=827846
|
||||
// for the documentation about the extensions.json format
|
||||
"recommendations": [
|
||||
"platformio.platformio-ide"
|
||||
],
|
||||
"unwantedRecommendations": [
|
||||
"ms-vscode.cpptools-extension-pack",
|
||||
"pioarduino.pioarduino-ide"
|
||||
]
|
||||
}
|
||||
|
|
@ -0,0 +1,37 @@
|
|||
|
||||
This directory is intended for project header files.
|
||||
|
||||
A header file is a file containing C declarations and macro definitions
|
||||
to be shared between several project source files. You request the use of a
|
||||
header file in your project source file (C, C++, etc) located in `src` folder
|
||||
by including it, with the C preprocessing directive `#include'.
|
||||
|
||||
```src/main.c
|
||||
|
||||
#include "header.h"
|
||||
|
||||
int main (void)
|
||||
{
|
||||
...
|
||||
}
|
||||
```
|
||||
|
||||
Including a header file produces the same results as copying the header file
|
||||
into each source file that needs it. Such copying would be time-consuming
|
||||
and error-prone. With a header file, the related declarations appear
|
||||
in only one place. If they need to be changed, they can be changed in one
|
||||
place, and programs that include the header file will automatically use the
|
||||
new version when next recompiled. The header file eliminates the labor of
|
||||
finding and changing all the copies as well as the risk that a failure to
|
||||
find one copy will result in inconsistencies within a program.
|
||||
|
||||
In C, the convention is to give header files names that end with `.h'.
|
||||
|
||||
Read more about using header files in official GCC documentation:
|
||||
|
||||
* Include Syntax
|
||||
* Include Operation
|
||||
* Once-Only Headers
|
||||
* Computed Includes
|
||||
|
||||
https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
|
||||
This directory is intended for project specific (private) libraries.
|
||||
PlatformIO will compile them to static libraries and link into the executable file.
|
||||
|
||||
The source code of each library should be placed in a separate directory
|
||||
("lib/your_library_name/[Code]").
|
||||
|
||||
For example, see the structure of the following example libraries `Foo` and `Bar`:
|
||||
|
||||
|--lib
|
||||
| |
|
||||
| |--Bar
|
||||
| | |--docs
|
||||
| | |--examples
|
||||
| | |--src
|
||||
| | |- Bar.c
|
||||
| | |- Bar.h
|
||||
| | |- library.json (optional. for custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
|
||||
| |
|
||||
| |--Foo
|
||||
| | |- Foo.c
|
||||
| | |- Foo.h
|
||||
| |
|
||||
| |- README --> THIS FILE
|
||||
|
|
||||
|- platformio.ini
|
||||
|--src
|
||||
|- main.c
|
||||
|
||||
Example contents of `src/main.c` using Foo and Bar:
|
||||
```
|
||||
#include <Foo.h>
|
||||
#include <Bar.h>
|
||||
|
||||
int main (void)
|
||||
{
|
||||
...
|
||||
}
|
||||
|
||||
```
|
||||
|
||||
The PlatformIO Library Dependency Finder will find automatically dependent
|
||||
libraries by scanning project source files.
|
||||
|
||||
More information about PlatformIO Library Dependency Finder
|
||||
- https://docs.platformio.org/page/librarymanager/ldf.html
|
||||
|
|
@ -0,0 +1,21 @@
|
|||
; PlatformIO Project Configuration File
|
||||
;
|
||||
; Build options: build flags, source filter
|
||||
; Upload options: custom upload port, speed and extra flags
|
||||
; Library options: dependencies, extra library storages
|
||||
; Advanced options: extra scripting
|
||||
;
|
||||
; Please visit documentation for the other options and examples
|
||||
; https://docs.platformio.org/page/projectconf.html
|
||||
|
||||
[env:esp32dev]
|
||||
platform = espressif32
|
||||
board = esp32dev
|
||||
framework = arduino
|
||||
monitor_speed = 115200
|
||||
lib_compat_mode = soft
|
||||
lib_deps =
|
||||
jrowberg/I2Cdevlib-Core
|
||||
jrowberg/I2Cdevlib-MPU6050
|
||||
Adafruit_MPU6050
|
||||
Adafruit_Sensor
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
{
|
||||
"folders": [
|
||||
{
|
||||
"path": "../../../Arduino/sketch_oct8a"
|
||||
},
|
||||
{
|
||||
"name": "TwoFootBot-arduion",
|
||||
"path": "."
|
||||
}
|
||||
],
|
||||
"settings": {}
|
||||
}
|
||||
|
|
@ -0,0 +1,81 @@
|
|||
#include "I2Cdev.h"
|
||||
#include "MPU6050_6Axis_MotionApps20.h"
|
||||
#include "Wire.h"
|
||||
#include <MPU6050.h>
|
||||
#include "imu.h"
|
||||
|
||||
MPU6050 mpu;
|
||||
bool dmpReady = false;
|
||||
uint8_t fifoBuffer[64];
|
||||
|
||||
Quaternion q;
|
||||
VectorFloat gravity;
|
||||
|
||||
// ===== 初始化 =====
|
||||
void imu_setup() {
|
||||
Serial.begin(115200);
|
||||
Wire.begin(21, 22);
|
||||
|
||||
mpu.initialize();
|
||||
if (!mpu.testConnection()) {
|
||||
Serial.println("MPU6050 连接失败");
|
||||
while (1);
|
||||
}
|
||||
|
||||
uint8_t devStatus = mpu.dmpInitialize();
|
||||
if (devStatus == 0) {
|
||||
mpu.CalibrateAccel(6);
|
||||
mpu.CalibrateGyro(6);
|
||||
mpu.setDMPEnabled(true);
|
||||
dmpReady = true;
|
||||
Serial.println("DMP 就绪");
|
||||
} else {
|
||||
Serial.print("DMP 初始化失败，错误码: ");
|
||||
Serial.println(devStatus);
|
||||
}
|
||||
}
|
||||
|
||||
// ===== DMP 读取，返回 YPR（角度制）=====
|
||||
YPR dmp() {
|
||||
YPR out = {0, 0, 0};
|
||||
if (!dmpReady) return out;
|
||||
|
||||
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;
|
||||
}
|
||||
// Serial.print("Yaw: "); Serial.print(a.yaw);
|
||||
// Serial.print(" Pitch: "); Serial.print(a.pitch);
|
||||
// Serial.print(" Roll: "); Serial.println(a.roll);
|
||||
return out;
|
||||
}
|
||||
|
||||
// ===== 原始数据读取，返回 RawData =====
|
||||
RawData raw() {
|
||||
RawData r;
|
||||
mpu.getMotion6(&r.ax, &r.ay, &r.az, &r.gx, &r.gy, &r.gz);
|
||||
|
||||
// ±2g 量程下 1g = 16384 LSB
|
||||
// r.ax_g = r.ax / 16384.0f;
|
||||
// r.ay_g = r.ay / 16384.0f;
|
||||
// r.az_g = r.az / 16384.0f;
|
||||
|
||||
// // ±250°/s 量程下 1°/s = 131 LSB
|
||||
// r.gx_dps = r.gx / 131.0f;
|
||||
// r.gy_dps = r.gy / 131.0f;
|
||||
// r.gz_dps = r.gz / 131.0f;
|
||||
// Serial.print("Accel(g): X="); Serial.print(r.ax_g, 3);
|
||||
// Serial.print(" Y="); Serial.print(r.ay_g, 3);
|
||||
// Serial.print(" Z="); Serial.print(r.az_g, 3);
|
||||
// Serial.print(" | Gyro(°/s): X="); Serial.print(r.gx_dps, 2);
|
||||
// Serial.print(" Y="); Serial.print(r.gy_dps, 2);
|
||||
// Serial.print(" Z="); Serial.println(r.gz_dps, 2);
|
||||
|
||||
return r;
|
||||
}
|
||||
|
|
@ -0,0 +1,27 @@
|
|||
#ifndef IMU_DATA_H
|
||||
#define IMU_DATA_H
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
// 返回姿态角（单位：度）
|
||||
struct YPR {
|
||||
float yaw;
|
||||
float pitch;
|
||||
float roll;
|
||||
};
|
||||
|
||||
// 返回原始数据及换算值
|
||||
struct RawData {
|
||||
int16_t ax, ay, az;
|
||||
int16_t gx, gy, gz;
|
||||
// float ax_g, ay_g, az_g;
|
||||
// float gx_dps, gy_dps, gz_dps;
|
||||
//
|
||||
};
|
||||
|
||||
// 函数声明
|
||||
YPR dmp();
|
||||
RawData raw();
|
||||
void imu_setup();
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,25 @@
|
|||
|
||||
#include "imu/imu.h"
|
||||
#include "wifi/wifi-sg90.h"
|
||||
#include <esp32-hal.h>
|
||||
|
||||
void setup() {
|
||||
// 串口初始化等公共配置
|
||||
Serial.begin(115200);
|
||||
|
||||
// 调用子模块的初始化函数
|
||||
imu_setup();
|
||||
wifi_setup();
|
||||
}
|
||||
void loop() {
|
||||
static unsigned long lastTelemetry = 0;
|
||||
unsigned long now = millis();
|
||||
//unsigned long now = micros(); // 改用微秒级计时
|
||||
|
||||
if (now - lastTelemetry >= 300) {
|
||||
lastTelemetry = now;
|
||||
YPR a = dmp();
|
||||
RawData r = raw();
|
||||
sendLineData(a, r);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
#pragma once
|
||||
#ifndef WIFI_SG90_H
|
||||
#define WIFI_SG90_H
|
||||
|
||||
#include "../imu/imu.h"
|
||||
|
||||
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);
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,381 @@
|
|||
#include <WiFi.h>
|
||||
#include <freertos/FreeRTOS.h>
|
||||
#include <freertos/queue.h>
|
||||
#include <math.h>
|
||||
#include "wifi/wifi-sg90.h"
|
||||
|
||||
// ==================== WiFi / TCP 配置 ====================
|
||||
const char* AP_SSID = "ESP32_AP";
|
||||
const char* AP_PASS = "12345678";
|
||||
const int TCP_PORT = 3333;
|
||||
|
||||
// ==================== 舵机配置 ====================
|
||||
const uint8_t NUM_SERVOS = 8;
|
||||
// 注意：避开 GPIO 6~11（Flash）、34~39（仅输入）、2/15（启动脚）
|
||||
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 float RAD_MIN = 0.0f;
|
||||
const float RAD_MAX = PI;
|
||||
|
||||
// ==================== 状态 ====================
|
||||
struct ServoState {
|
||||
uint8_t pin;
|
||||
float targetRad;
|
||||
unsigned long lastCycle;
|
||||
};
|
||||
|
||||
ServoState g_servos[NUM_SERVOS];
|
||||
|
||||
WiFiServer tcpServer(TCP_PORT);
|
||||
WiFiClient client;
|
||||
|
||||
struct TelemetryData {
|
||||
YPR attitude;
|
||||
RawData raw;
|
||||
};
|
||||
|
||||
QueueHandle_t telemetryQueue = nullptr;
|
||||
char commandBuffer[128];
|
||||
size_t commandLength = 0;
|
||||
bool commandOverflow = false;
|
||||
|
||||
// ==================== 前向声明 ====================
|
||||
bool isNumeric(const String& s);
|
||||
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);
|
||||
void writeInt16BE(uint8_t* buffer, size_t offset, int16_t value);
|
||||
uint16_t calculateCrc16(const uint8_t* data, size_t length);
|
||||
|
||||
// ==================== 舵机初始化 / 更新 ====================
|
||||
|
||||
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);
|
||||
|
||||
pinMode(SERVO_PINS[i], OUTPUT);
|
||||
digitalWrite(SERVO_PINS[i], LOW);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @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) *
|
||||
(rad / PI) + 0.5f);
|
||||
|
||||
digitalWrite(s.pin, HIGH);
|
||||
delayMicroseconds(pulse);
|
||||
digitalWrite(s.pin, LOW);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ==================== WiFi / TCP ====================
|
||||
|
||||
void initWiFiAP() {
|
||||
WiFi.mode(WIFI_AP);
|
||||
WiFi.softAP(AP_SSID, AP_PASS);
|
||||
Serial.println("==== WiFi AP 已启动 ====");
|
||||
Serial.print("SSID : "); Serial.println(AP_SSID);
|
||||
Serial.print("Password: "); Serial.println(AP_PASS);
|
||||
Serial.print("AP IP : "); Serial.println(WiFi.softAPIP());
|
||||
}
|
||||
|
||||
void initTcpServer() {
|
||||
tcpServer.begin();
|
||||
tcpServer.setNoDelay(true);
|
||||
Serial.printf("TCP 服务器已启动，端口: %d\n", TCP_PORT);
|
||||
}
|
||||
|
||||
void sendLine(const String& msg) {
|
||||
if (client && client.connected()) {
|
||||
client.println(msg);
|
||||
}
|
||||
}
|
||||
|
||||
void acceptClientIfNeeded() {
|
||||
if (!client || !client.connected()) {
|
||||
client = tcpServer.available();
|
||||
if (client) {
|
||||
commandLength = 0;
|
||||
commandOverflow = false;
|
||||
Serial.print("上位机已连接: ");
|
||||
Serial.println(client.remoteIP());
|
||||
sendLine("ESP32: 已连接。请发送 8 个角度，例如 "
|
||||
"RAD:1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57");
|
||||
} else {
|
||||
commandLength = 0;
|
||||
commandOverflow = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ==================== 解析工具 ====================
|
||||
|
||||
bool isNumeric(const String& s) {
|
||||
if (s.length() == 0) return false;
|
||||
bool dotSeen = false;
|
||||
for (size_t i = 0; i < s.length(); i++) {
|
||||
char c = s.charAt(i);
|
||||
if (c == '-' || c == '+') {
|
||||
if (i != 0) return false;
|
||||
} else if (c == '.') {
|
||||
if (dotSeen) return false;
|
||||
dotSeen = true;
|
||||
} else if (!isDigit(c)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 解析逗号分隔的浮点列表
|
||||
* @return 成功解析出的元素个数（遇到非法 token 立即停止）
|
||||
*/
|
||||
uint8_t parseFloatList(const String& s, float* out, uint8_t maxCount) {
|
||||
uint8_t count = 0;
|
||||
int start = 0;
|
||||
int len = s.length();
|
||||
|
||||
while (start < len && count < maxCount) {
|
||||
int comma = s.indexOf(',', start);
|
||||
String token;
|
||||
if (comma < 0) {
|
||||
token = s.substring(start);
|
||||
start = len;
|
||||
} else {
|
||||
token = s.substring(start, comma);
|
||||
start = comma + 1;
|
||||
}
|
||||
token.trim();
|
||||
if (token.length() == 0) continue;
|
||||
|
||||
if (!isNumeric(token)) break; // 非法 token -> 放弃后续
|
||||
out[count++] = token.toFloat();
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
// ==================== 指令处理 ====================
|
||||
|
||||
void sendCurrentState() {
|
||||
String msg = "ESP32:";
|
||||
for (uint8_t i = 0; i < NUM_SERVOS; i++) {
|
||||
msg += " S";
|
||||
msg += String(i);
|
||||
msg += "=";
|
||||
msg += String(g_servos[i].targetRad, 4);
|
||||
}
|
||||
sendLine(msg);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 支持指令：
|
||||
* RAD:r0,r1,...,r7 弧度值列表
|
||||
* DEG:d0,d1,...,d7 角度值列表（内部转弧度）
|
||||
* r0,r1,...,r7 纯数字按弧度处理
|
||||
* GET 查询当前 8 路角度
|
||||
* PING 心跳
|
||||
*/
|
||||
void processCommand(const String& raw) {
|
||||
String cmd = raw;
|
||||
cmd.trim();
|
||||
if (cmd.length() == 0) return;
|
||||
|
||||
if (cmd == "PING") { sendLine("ESP32: PONG"); return; }
|
||||
if (cmd == "GET") { sendCurrentState(); return; }
|
||||
|
||||
float values[NUM_SERVOS];
|
||||
uint8_t n = 0;
|
||||
bool inDeg = false;
|
||||
|
||||
if (cmd.startsWith("RAD:")) {
|
||||
n = parseFloatList(cmd.substring(4), values, NUM_SERVOS);
|
||||
} else if (cmd.startsWith("DEG:")) {
|
||||
n = parseFloatList(cmd.substring(4), values, NUM_SERVOS);
|
||||
inDeg = true;
|
||||
} else if (cmd.startsWith("SET:")) {
|
||||
n = parseFloatList(cmd.substring(4), values, NUM_SERVOS);
|
||||
} else {
|
||||
n = parseFloatList(cmd, values, NUM_SERVOS);
|
||||
}
|
||||
|
||||
if (n == 0) {
|
||||
sendLine("ESP32: 无法解析 -> " + cmd);
|
||||
return;
|
||||
}
|
||||
|
||||
// 写入目标角度
|
||||
for (uint8_t i = 0; i < n && i < NUM_SERVOS; i++) {
|
||||
float rad = inDeg ? (values[i] * PI / 180.0f) : values[i];
|
||||
if (rad < RAD_MIN) rad = RAD_MIN;
|
||||
if (rad > RAD_MAX) rad = RAD_MAX;
|
||||
g_servos[i].targetRad = rad;
|
||||
}
|
||||
|
||||
// 简单日志（不要频繁打印，避免影响时序）
|
||||
Serial.printf("SET %u servo(s)\n", n);
|
||||
|
||||
sendLine("ESP32: OK 设置 " + String(n) + " 个舵机");
|
||||
}
|
||||
|
||||
void handleClientCommand() {
|
||||
if (!client || !client.connected()) return;
|
||||
|
||||
uint16_t bytesRead = 0;
|
||||
while (client.available() && bytesRead < sizeof(commandBuffer)) {
|
||||
int incoming = client.read();
|
||||
if (incoming < 0) break;
|
||||
bytesRead++;
|
||||
|
||||
if (incoming == '\n') {
|
||||
if (commandOverflow) {
|
||||
sendLine("ESP32: 指令过长");
|
||||
} else {
|
||||
commandBuffer[commandLength] = '\0';
|
||||
processCommand(String(commandBuffer));
|
||||
}
|
||||
commandLength = 0;
|
||||
commandOverflow = false;
|
||||
} else if (!commandOverflow) {
|
||||
if (commandLength < sizeof(commandBuffer) - 1) {
|
||||
commandBuffer[commandLength++] = static_cast<char>(incoming);
|
||||
} else {
|
||||
commandOverflow = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void sendQueuedTelemetry() {
|
||||
if (!client || !client.connected()) {
|
||||
return;
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
if (client.write(packet, sizeof(packet)) != sizeof(packet)) {
|
||||
Serial.println("警告：IMU 二进制数据发送不完整");
|
||||
}
|
||||
}
|
||||
|
||||
int16_t encodeAngle(float degrees) {
|
||||
long scaled = lroundf(degrees * 100.0f);
|
||||
if (scaled > 32767L) scaled = 32767L;
|
||||
if (scaled < -32768L) scaled = -32768L;
|
||||
return static_cast<int16_t>(scaled);
|
||||
}
|
||||
|
||||
void writeInt16BE(uint8_t* buffer, size_t offset, int16_t value) {
|
||||
uint16_t encoded = static_cast<uint16_t>(value);
|
||||
buffer[offset] = static_cast<uint8_t>(encoded >> 8);
|
||||
buffer[offset + 1] = static_cast<uint8_t>(encoded);
|
||||
}
|
||||
|
||||
uint16_t calculateCrc16(const uint8_t* data, size_t length) {
|
||||
uint16_t crc = 0xFFFF;
|
||||
for (size_t i = 0; i < length; i++) {
|
||||
crc ^= static_cast<uint16_t>(data[i]) << 8;
|
||||
for (uint8_t bit = 0; bit < 8; bit++) {
|
||||
crc = (crc & 0x8000)
|
||||
? static_cast<uint16_t>((crc << 1) ^ 0x1021)
|
||||
: static_cast<uint16_t>(crc << 1);
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
void sg90Task(void* parameter) {
|
||||
(void)parameter;
|
||||
for (;;) {
|
||||
sg90();
|
||||
sendQueuedTelemetry();
|
||||
vTaskDelay(1);
|
||||
}
|
||||
}
|
||||
void sendLineData(YPR a, RawData r) {
|
||||
if (telemetryQueue == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
TelemetryData data = {a, r};
|
||||
xQueueOverwrite(telemetryQueue, &data);
|
||||
}
|
||||
|
||||
// ==================== Arduino 主入口 ====================
|
||||
|
||||
void wifi_setup() {
|
||||
Serial.begin(115200);
|
||||
servoInit();
|
||||
initWiFiAP();
|
||||
initTcpServer();
|
||||
|
||||
telemetryQueue = xQueueCreate(1, sizeof(TelemetryData));
|
||||
if (telemetryQueue == nullptr) {
|
||||
Serial.println("错误：无法创建 IMU 遥测队列");
|
||||
return;
|
||||
}
|
||||
|
||||
BaseType_t taskCreated = xTaskCreatePinnedToCore(
|
||||
sg90Task, "sg90", 4096, nullptr, 2, nullptr, 1);
|
||||
if (taskCreated != pdPASS) {
|
||||
Serial.println("错误：无法创建 SG90 通讯任务");
|
||||
vQueueDelete(telemetryQueue);
|
||||
telemetryQueue = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
void sg90() {
|
||||
acceptClientIfNeeded(); // 1) 接入客户端
|
||||
handleClientCommand(); // 2) 解析上位机指令
|
||||
servoUpdateAll(); // 3) 非阻塞更新 8 路 PWM
|
||||
}
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
|
||||
This directory is intended for PlatformIO Test Runner and project tests.
|
||||
|
||||
Unit Testing is a software testing method by which individual units of
|
||||
source code, sets of one or more MCU program modules together with associated
|
||||
control data, usage procedures, and operating procedures, are tested to
|
||||
determine whether they are fit for use. Unit testing finds problems early
|
||||
in the development cycle.
|
||||
|
||||
More information about PlatformIO Unit Testing:
|
||||
- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html
|
||||
Loading…
Reference in New Issue