零知開源——基于STM32F407VET6和ADXL345三軸加速度計的精準運動姿態檢測系統

??零知IDE 是一個真正屬于國人自己的開源軟件平臺，在開發效率上超越了Arduino平臺并且更加容易上手，大大降低了開發難度。零知開源在軟件方面提供了完整的學習教程和豐富示例代碼，讓不懂程序的工程師也能非常輕而易舉的搭建電路來創作產品，測試產品。快來動手試試吧！

?訪問零知實驗室，獲取更多實戰項目和教程資源吧！

www.lingzhilab.com

一、硬件設計部分

1.1?硬件清單

1.2 接線方案

1.3?連接硬件圖

1.4 接線實物圖

二、軟件架構部分

2.1 初始化結構

2.2 卡爾曼濾波

2.3 平均濾波及校準算法

2.4?Processing可視化代碼

2.5 加速度計完整代碼

三、校準過程及展示

3.1 校準過程

3.2 上位機界面展示

3.3 視頻演示

四、ADXL345寄存器工作原理

4.1?DATA_FORMAT寄存器(0x31)

4.2?BW_RATE寄存器(0x2C)

4.3?POWER_CTL寄存器(0x2D)

4.4 加速度計I2C通信

五、常見問題解答

Q1: 傳感器數據出現NaN值怎么辦？

Q2: 如何提高角度計算精度？

Q3: 為什么三維立方體顯示不全或位置不正確？

（1）項目概述

????????本項目基于零知增強板（主控芯片STM32F407VET6）和ADXL345三軸加速度計，開發了一套完整的實時姿態監測系統。通過先進的濾波算法和自動校準技術，實現了高精度的姿態角度解算，并通過Processing平臺實現了三維可視化界面。該系統能夠實時顯示設備的滾轉、俯仰角度，并記錄歷史數據變化趨勢。

（2）項目亮點

? ? ? ? >結合卡爾曼濾波和移動平均濾波，有效降低傳感器噪聲
? ? ? ? >智能六點校準算法，無需手動干預
? ? ? ? >Processing三維實時顯示，支持歷史數據回溯

（3）項目難點及解決方案

? ? ? ?問題描述：傳感器存在明顯噪聲和動態漂移

解決方案：采用雙重濾波算法（卡爾曼濾波+移動平均濾波），設置合理的濾波參數，有效抑制噪聲同時保持響應速度。

一、硬件設計部分

1.1?硬件清單

組件名稱	規格型號	數量	備注
零知增強板	STM32F407VET6主控	1	核心控制器
ADXL345模塊	三軸加速度計	1	姿態傳感器
杜邦線	20cm	4	連接線材
微型USB線	/	1	供電與編程

1.2 接線方案

ADXL345引腳	零知增強板引腳	功能說明
VCC	5V	電源正極
GND	GND	電源地
SDA	20/SDA	I2C數據線
SCL	21/SCL	I2C時鐘線

1.3?連接硬件圖

????????ADXL345模塊通過I2C接口與零知增強板連接，使用5V供電

1.4 接線實物圖

二、軟件架構部分

2.1 初始化結構

#include <Wire.h>// ADXL345 I2C地址
#define ADXL345_ADDRESS 0x53// ADXL345寄存器地址
#define ADXL345_REG_DEVID 0x00
#define ADXL345_REG_POWER_CTL 0x2D
#define ADXL345_REG_DATA_FORMAT 0x31
#define ADXL345_REG_DATAX0 0x32
#define ADXL345_REG_BW_RATE 0x2C
#define ADXL345_REG_OFSX 0x1E
#define ADXL345_REG_OFSY 0x1F
#define ADXL345_REG_OFSZ 0x20// 高級濾波參數
#define MOVING_AVG_SIZE 7    // 移動平均窗口大小
#define KALMAN_Q 0.01        // 卡爾曼濾波過程噪聲
#define KALMAN_R 0.1         // 卡爾曼濾波測量噪聲
#define STABILITY_THRESHOLD 0.02 // 穩定性閾值(g)// 校準參數
float xOffset = 0, yOffset = 0, zOffset = 0;
bool calibrated = false;// 濾波結構體
typedef struct {float q; // 過程噪聲協方差float r; // 測量噪聲協方差float x; // 估計值float p; // 估計誤差協方差float k; // 卡爾曼增益
} KalmanFilter;KalmanFilter kalmanX, kalmanY, kalmanZ;// ADXL345初始化函數
void initADXL345() {Wire.begin();// 檢查設備IDbyte deviceID = readRegister(ADXL345_REG_DEVID);if (deviceID != 0xE5) {Serial.println("Error: ADXL345 not found");while(1);}// 設置數據速率和帶寬為100HzwriteRegister(ADXL345_REG_BW_RATE, 0x0B);// 設置數據格式為±4g，全分辨率模式writeRegister(ADXL345_REG_DATA_FORMAT, 0x09);// 啟用測量模式writeRegister(ADXL345_REG_POWER_CTL, 0x08);Serial.println("ADXL345 initialized successfully");
}

????????MOVING_AVG_SIZE = 7：移動平均窗口大小，影響平滑度和響應速度
????????STABILITY_THRESHOLD = 0.02：穩定性判斷閾值，小于此值認為設備穩定

2.2 卡爾曼濾波

// 移動平均濾波
float movingAvgBufferX[MOVING_AVG_SIZE];
float movingAvgBufferY[MOVING_AVG_SIZE];
float movingAvgBufferZ[MOVING_AVG_SIZE];
int movingAvgIndex = 0;// 穩定性檢測
unsigned long lastStableTime = 0;
bool isStable = false;// 初始化卡爾曼濾波器
void initKalmanFilter(KalmanFilter *kf, float q, float r, float initialValue) {kf->q = q;kf->r = r;kf->x = initialValue;kf->p = 1.0;kf->k = 0;
}// 卡爾曼濾波更新
float updateKalmanFilter(KalmanFilter *kf, float measurement) {// 預測kf->p = kf->p + kf->q;// 更新kf->k = kf->p / (kf->p + kf->r);kf->x = kf->x + kf->k * (measurement - kf->x);kf->p = (1 - kf->k) * kf->p;return kf->x;
}// 應用卡爾曼濾波
void applyKalmanFilter(float &x, float &y, float &z) {x = updateKalmanFilter(&kalmanX, x);y = updateKalmanFilter(&kalmanY, y);z = updateKalmanFilter(&kalmanZ, z);
}

2.3 平均濾波及校準算法

// 應用移動平均濾波
void applyMovingAverageFilter(float &x, float &y, float &z) {// 更新緩沖區movingAvgBufferX[movingAvgIndex] = x;movingAvgBufferY[movingAvgIndex] = y;movingAvgBufferZ[movingAvgIndex] = z;movingAvgIndex = (movingAvgIndex + 1) % MOVING_AVG_SIZE;// 計算平均值float xSum = 0, ySum = 0, zSum = 0;for (int i = 0; i < MOVING_AVG_SIZE; i++) {xSum += movingAvgBufferX[i];ySum += movingAvgBufferY[i];zSum += movingAvgBufferZ[i];}x = xSum / MOVING_AVG_SIZE;y = ySum / MOVING_AVG_SIZE;z = zSum / MOVING_AVG_SIZE;
}// 自動校準函數
void autoCalibrate() {Serial.println("Start automatic calibration...");Serial.println("Please ensure that the sensor is placed horizontally and stationary");// 等待2秒讓用戶放置傳感器delay(2000);float xSum = 0, ySum = 0, zSum = 0;const int samples = 200;int validSamples = 0;// 采集多個樣本求平均值for (int i = 0; i < samples; i++) {float x, y, z;readAcceleration(x, y, z);// 檢查數據有效性if (!isnan(x) && !isnan(y) && !isnan(z)) {xSum += x;ySum += y;zSum += z;validSamples++;}delay(10); // 每10ms采集一次}if (validSamples > 0) {// 計算偏移量xOffset = xSum / validSamples;yOffset = ySum / validSamples;zOffset = zSum / validSamples - 1.0; // 減去重力加速度// 設置硬件偏移寄存器writeRegister(ADXL345_REG_OFSX, (byte)(xOffset / 0.0156));writeRegister(ADXL345_REG_OFSY, (byte)(yOffset / 0.0156));writeRegister(ADXL345_REG_OFSZ, (byte)(zOffset / 0.0156));calibrated = true;Serial.println("Automatic calibration completed!");Serial.print("XOffset: "); Serial.print(xOffset, 6);Serial.print(" g, YOffset: "); Serial.print(yOffset, 6);Serial.print(" g, ZOffset: "); Serial.print(zOffset, 6);Serial.println(" g");} else {Serial.println("Calibration failure: No valid data can be obtained");}
}// 處理串口命令
void processSerialCommands() {if (Serial.available()) {char command = Serial.read();if (command == 'c' || command == 'C') {autoCalibrate();} else if (command == 'r' || command == 'R') {// 重置校準數據calibrated = false;xOffset = 0;yOffset = 0;zOffset = 0;writeRegister(ADXL345_REG_OFSX, 0);writeRegister(ADXL345_REG_OFSY, 0);writeRegister(ADXL345_REG_OFSZ, 0);Serial.println("The calibration data has been reset");} else if (command == 's' || command == 'S') {// 顯示狀態Serial.print("校準狀態 : ");Serial.println(calibrated ? "Calibrated Resistance" : "Uncalibrated");Serial.print("XOffset: "); Serial.println(xOffset, 6);Serial.print("YOffset: "); Serial.println(yOffset, 6);Serial.print("ZOffset: "); Serial.println(zOffset, 6);}}
}

2.4?Processing可視化代碼

import processing.serial.*;// 傳感器數據
float roll, pitch, yaw;
PVector accelerometer = new PVector();
PVector gyroscope = new PVector(0, 0, 0);
PVector magneticField = new PVector(0, 0, 0);// 3D模型
PShape model;
PImage bgImage;// 串口配置
Serial port;
String[] serialPorts;
int selectedPort = 0;
boolean printSerial = true;
boolean connected = false;// 歷史數據
float[] rollHistory = new float[200];
float[] pitchHistory = new float[200];
int historyIndex = 0;// UI顏色主題
color backgroundColor = color(30, 30, 40);
color panelColor = color(40, 40, 50, 200);
color textColor = color(220, 220, 220);
color accentColor = color(65, 105, 225); // 皇家藍
color rollColor = color(220, 80, 80);    // 紅色
color pitchColor = color(80, 220, 80);   // 綠色
color yawColor = color(80, 80, 220);     // 藍色// 字體
PFont dataFont, titleFont;void setup() {size(1400, 900, P3D);frameRate(60);// 創建字體dataFont = createFont("Arial", 16);titleFont = createFont("Arial Bold", 20);// 創建默認背景bgImage = createGradientBackground();// 創建默認模型 - 更大的立方體model = createEnhancedCube();// 初始化串口serialPorts = Serial.list();if (serialPorts.length > 0) {connectSerial(serialPorts[0]);}// 初始化歷史數據for (int i = 0; i < rollHistory.length; i++) {rollHistory[i] = 0;pitchHistory[i] = 0;}println("等待數據...");
}PImage createGradientBackground() {PGraphics pg = createGraphics(width, height);pg.beginDraw();pg.background(backgroundColor);// 添加漸變效果for (int i = 0; i < height; i++) {float inter = map(i, 0, height, 0, 1);color c = lerpColor(color(20, 20, 30), color(40, 40, 60), inter);pg.stroke(c);pg.line(0, i, width, i);}// 添加網格線pg.stroke(50, 100);for (int x = 0; x < width; x += 50) {pg.line(x, 0, x, height);}for (int y = 0; y < height; y += 50) {pg.line(0, y, width, y);}pg.endDraw();return pg.get();
}PShape createEnhancedCube() {// 創建一個更詳細、更大的立方體模型PShape cube = createShape(GROUP);// 立方體主體 (100x100x100)PShape mainBody = createShape(BOX, 150, 150, 150);mainBody.setFill(color(180, 100, 100, 200));mainBody.setStroke(false);cube.addChild(mainBody);// 添加邊緣高光PShape edges = createShape();edges.beginShape(LINES);edges.stroke(255, 150);edges.strokeWeight(2);// 立方體邊緣float s = 75; // 半邊長edges.vertex(-s, -s, -s); edges.vertex(s, -s, -s);edges.vertex(-s, -s, -s); edges.vertex(-s, s, -s);edges.vertex(-s, -s, -s); edges.vertex(-s, -s, s);edges.vertex(s, s, s); edges.vertex(-s, s, s);edges.vertex(s, s, s); edges.vertex(s, -s, s);edges.vertex(s, s, s); edges.vertex(s, s, -s);edges.vertex(-s, s, -s); edges.vertex(s, s, -s);edges.vertex(-s, s, -s); edges.vertex(-s, s, s);edges.vertex(s, -s, -s); edges.vertex(s, s, -s);edges.vertex(s, -s, -s); edges.vertex(s, -s, s);edges.vertex(-s, -s, s); edges.vertex(s, -s, s);edges.vertex(-s, -s, s); edges.vertex(-s, s, s);edges.endShape();cube.addChild(edges);// 添加方向指示器PShape xIndicator = createShape(SPHERE, 10);xIndicator.setFill(rollColor);xIndicator.setStroke(false);xIndicator.translate(85, 0, 0);cube.addChild(xIndicator);PShape yIndicator = createShape(SPHERE, 10);yIndicator.setFill(pitchColor);yIndicator.setStroke(false);yIndicator.translate(0, 85, 0);cube.addChild(yIndicator);PShape zIndicator = createShape(SPHERE, 10);zIndicator.setFill(yawColor);zIndicator.setStroke(false);zIndicator.translate(0, 0, 85);cube.addChild(zIndicator);return cube;
}void draw() {// 繪制背景image(bgImage, 0, 0);// 繪制3D場景 (占據左側大部分區域)draw3DScene();// 繪制數據面板 (右側)drawDataPanel();// 繪制波形圖 (右下角)drawWaveformGraph();// 繪制連接狀態drawConnectionStatus();
}void draw3DScene() {pushMatrix();// 將3D場景移到左側中心，占據更大空間translate(width/3, height/2, 0);// 設置燈光 - 增強照明lights();directionalLight(150, 150, 150, 0, 0, -1);directionalLight(100, 100, 100, 0, 1, 0);directionalLight(80, 80, 80, 1, 0, 0);lightSpecular(255, 255, 255);shininess(20.0);// 應用旋轉 - 調整Z軸方向// 注意: Processing默認Y向上，我們需要調整使Z向上rotateX(radians(0)); // 先旋轉使Z軸向上rotateZ(radians(-roll));rotateX(radians(-pitch));rotateY(radians(-yaw)); // 調整yaw旋轉方向// 繪制參考坐標系 (更大)drawCoordinateSystem();// 繪制模型shape(model);popMatrix();
}void drawCoordinateSystem() {float axisLength = 120;// 繪制X軸 (紅色)strokeWeight(3);stroke(rollColor);line(0, 0, 0, axisLength, 0, 0);pushMatrix();translate(axisLength + 15, 0, 0);fill(rollColor);textFont(titleFont);text("Y", 0, 0);popMatrix();// 繪制Y軸 (綠色)stroke(pitchColor);line(0, 0, 0, 0, axisLength, 0);pushMatrix();translate(0, axisLength + 15, 0);fill(pitchColor);text("Z", 0, 0);popMatrix();// 繪制Z軸 (藍色) - 現在向上stroke(yawColor);line(0, 0, 0, 0, 0, axisLength);pushMatrix();translate(0, 0, axisLength + 15);fill(yawColor);text("X", 0, 0);popMatrix();strokeWeight(1);
}void drawDataPanel() {// 繪制數據面板背景 (右側)float panelWidth = 400;float panelX = width - panelWidth - 20;fill(panelColor);noStroke();rect(panelX, 20, panelWidth, 280, 10);// 面板標題fill(accentColor);textFont(titleFont);textAlign(LEFT, TOP);text("Sensor Data Panel", panelX + 15, 30);// 顯示傳感器數據textFont(dataFont);fill(textColor);String accelX = Float.isNaN(accelerometer.x) ? "N/A" : nfp(accelerometer.x, 1, 3);String accelY = Float.isNaN(accelerometer.y) ? "N/A" : nfp(accelerometer.y, 1, 3);String accelZ = Float.isNaN(accelerometer.z) ? "N/A" : nfp(accelerometer.z, 1, 3);String yawStr = Float.isNaN(yaw) ? "N/A" : nfp(yaw, 1, 1);String pitchStr = Float.isNaN(pitch) ? "N/A" : nfp(pitch, 1, 1);String rollStr = Float.isNaN(roll) ? "N/A" : nfp(roll, 1, 1);// 使用表格形式顯示數據float startY = 70;float rowHeight = 30;// 表頭fill(accentColor);text("parameter:", panelX + 20, startY);text("value:", panelX + 200, startY);// 加速度數據drawDataRow("accelerate X:", accelX + " g", startY + rowHeight, rollColor);drawDataRow("accelerate Y:", accelY + " g", startY + rowHeight * 2, pitchColor);drawDataRow("accelerate Z:", accelZ + " g", startY + rowHeight * 3, yawColor);// 姿態數據drawDataRow("Roll:", rollStr + "°", startY + rowHeight * 5, rollColor);drawDataRow("Pitch:", pitchStr + "°", startY + rowHeight * 6, pitchColor);drawDataRow("Yaw:", yawStr + "°", startY + rowHeight * 7, yawColor);
}void drawDataRow(String label, String value, float y, color c) {float panelX = width - 400 - 20;fill(textColor);text(label, panelX + 20, y);fill(c);text(value, panelX + 200, y);
}void drawWaveformGraph() {float graphX = width - 420;float graphY = 320;float graphWidth = 400;float graphHeight = 250;// 繪制波形圖背景fill(panelColor);noStroke();rect(graphX, graphY, graphWidth, graphHeight, 10);// 標題fill(accentColor);textFont(titleFont);text("Angle Waveform Diagram", graphX + 15, graphY + 15);// 繪制網格drawWaveformGrid(graphX, graphY, graphWidth, graphHeight);// 繪制零線stroke(150, 150);float zeroY = graphY + graphHeight / 2;line(graphX, zeroY, graphX + graphWidth, zeroY);// 繪制Roll歷史stroke(rollColor);drawWaveformLine(graphX, graphY, graphWidth, graphHeight, rollHistory, -90, 90);// 繪制Pitch歷史stroke(pitchColor);drawWaveformLine(graphX, graphY, graphWidth, graphHeight, pitchHistory, -90, 90);// 繪制標簽fill(textColor);textFont(dataFont);textAlign(LEFT);text("Roll", graphX, graphY + graphHeight + 40);fill(rollColor);rect(graphX + 40, graphY + graphHeight + 30, 15, 5);fill(textColor);text("Pitch", graphX, graphY + graphHeight + 70);fill(pitchColor);rect(graphX + 40, graphY + graphHeight + 62, 15, 5);// 繪制刻度fill(150);textAlign(CENTER);text("-90°", graphX, graphY + graphHeight + 20);text("0°", graphX + graphWidth / 2, graphY + graphHeight + 20);text("90°", graphX + graphWidth, graphY + graphHeight + 20);
}void drawWaveformGrid(float x, float y, float w, float h) {stroke(80, 100);// 水平網格線for (int i = 1; i < 4; i++) {float lineY = y + h * i / 4;line(x, lineY, x + w, lineY);}// 垂直網格線for (int i = 1; i < 5; i++) {float lineX = x + w * i / 5;line(lineX, y, lineX, y + h);}
}void drawWaveformLine(float x, float y, float w, float h, float[] history, float minVal, float maxVal) {noFill();beginShape();for (int i = 0; i < history.length; i++) {float xPos = x + map(i, 0, history.length - 1, 0, w);float yPos = y + h/2 - map(history[i], minVal, maxVal, -h/2, h/2);vertex(xPos, yPos);}endShape();
}void drawConnectionStatus() {float statusX = 20;float statusY = height - 80;// 連接狀態背景fill(panelColor);noStroke();rect(statusX, statusY, 300, 60, 8);// 連接狀態指示器fill(connected ? color(0, 200, 0) : color(200, 0, 0));ellipse(statusX + 20, statusY + 20, 15, 15);fill(textColor);textFont(dataFont);textAlign(LEFT, CENTER);text("UART: " + (connected ? serialPorts[selectedPort] : "No connection"), statusX + 45, statusY + 20);text("Data receiving: " + (millis() - lastDataTime < 1000 ? "Normal" : "No found"), statusX + 45, statusY + 40);// 指令提示text("Press the space bar to switch the serial port | C:Calibration | R:Reset", statusX, statusY + 70);
}void checkDataReceiving() {// 檢查數據接收狀態if (millis() - lastDataTime > 1000) {dataReceived = 0;}
}void serialEvent(Serial p) {try {String rawData = p.readStringUntil('\n');if (rawData == null) return;rawData = rawData.trim();if (printSerial) println("Raw: " + rawData);String[] parts = split(rawData, ' ');if (parts.length >= 4) {if (parts[0].equals("Or:")) {// 解析姿態數據: Or: yaw pitch rolltry {yaw = float(parts[1]);pitch = float(parts[2]);roll = float(parts[3]);// 檢查NaN值if (Float.isNaN(yaw)) yaw = 0;if (Float.isNaN(pitch)) pitch = 0;if (Float.isNaN(roll)) roll = 0;// 更新歷史數據rollHistory[historyIndex] = roll;pitchHistory[historyIndex] = pitch;historyIndex = (historyIndex + 1) % rollHistory.length;dataReceived |= 1;lastDataTime = millis();} catch (Exception e) {println("Error parsing orientation data: " + e.getMessage());}} else if (parts[0].equals("Accel:")) {// 解析加速度數據: Accel: x y ztry {float accelX = float(parts[1]);float accelY = float(parts[2]);float accelZ = float(parts[3]);// 檢查NaN值if (Float.isNaN(accelX)) accelX = 0;if (Float.isNaN(accelY)) accelY = 0;if (Float.isNaN(accelZ)) accelZ = 0;accelerometer.set(accelX, accelY, accelZ);dataReceived |= 2;lastDataTime = millis();} catch (Exception e) {println("Error parsing acceleration data: " + e.getMessage());}}}} catch(Exception e) {println("Serial Error: " + e.getMessage());}
}// 全局變量用于跟蹤數據接收狀態
int dataReceived = 0;
long lastDataTime = 0;void connectSerial(String portName) {if (port != null) {port.stop();}try {port = new Serial(this, portName, 115200);port.bufferUntil('\n');connected = true;println("Connected to: " + portName);} catch(Exception e) {println("Connection failed: " + e.getMessage());connected = false;}
}void keyPressed() {// 切換串口if (key == ' ') {if (serialPorts.length > 0) {selectedPort = (selectedPort + 1) % serialPorts.length;connectSerial(serialPorts[selectedPort]);}}// 切換調試輸出if (key == 'P' || key == 'p') {printSerial = !printSerial;println("Serial print: " + (printSerial ? "ON" : "OFF"));}// 重置視角if (key == 'R' || key == 'r') {yaw = pitch = roll = 0;println("View reset");}// 發送校準命令if (key == 'C' || key == 'c') {if (port != null) {port.write('c');println("Sent calibration command");}}
}

????????UI設計界面：左側3D實時渲染區域，右側數據展示、實時波形圖顯示歷史數據趨勢

2.5 加速度計完整代碼

#include <Wire.h>// ADXL345 I2C地址
#define ADXL345_ADDRESS 0x53// ADXL345寄存器地址
#define ADXL345_REG_DEVID 0x00
#define ADXL345_REG_POWER_CTL 0x2D
#define ADXL345_REG_DATA_FORMAT 0x31
#define ADXL345_REG_DATAX0 0x32
#define ADXL345_REG_BW_RATE 0x2C
#define ADXL345_REG_OFSX 0x1E
#define ADXL345_REG_OFSY 0x1F
#define ADXL345_REG_OFSZ 0x20// 高級濾波參數
#define MOVING_AVG_SIZE 7    // 移動平均窗口大小
#define KALMAN_Q 0.01        // 卡爾曼濾波過程噪聲
#define KALMAN_R 0.1         // 卡爾曼濾波測量噪聲
#define STABILITY_THRESHOLD 0.02 // 穩定性閾值(g)// 校準參數
float xOffset = 0, yOffset = 0, zOffset = 0;
bool calibrated = false;// 濾波結構體
typedef struct {float q; // 過程噪聲協方差float r; // 測量噪聲協方差float x; // 估計值float p; // 估計誤差協方差float k; // 卡爾曼增益
} KalmanFilter;KalmanFilter kalmanX, kalmanY, kalmanZ;// 移動平均濾波
float movingAvgBufferX[MOVING_AVG_SIZE];
float movingAvgBufferY[MOVING_AVG_SIZE];
float movingAvgBufferZ[MOVING_AVG_SIZE];
int movingAvgIndex = 0;// 穩定性檢測
unsigned long lastStableTime = 0;
bool isStable = false;// 初始化卡爾曼濾波器
void initKalmanFilter(KalmanFilter *kf, float q, float r, float initialValue) {kf->q = q;kf->r = r;kf->x = initialValue;kf->p = 1.0;kf->k = 0;
}// 卡爾曼濾波更新
float updateKalmanFilter(KalmanFilter *kf, float measurement) {// 預測kf->p = kf->p + kf->q;// 更新kf->k = kf->p / (kf->p + kf->r);kf->x = kf->x + kf->k * (measurement - kf->x);kf->p = (1 - kf->k) * kf->p;return kf->x;
}// ADXL345初始化函數
void initADXL345() {Wire.begin();// 檢查設備IDbyte deviceID = readRegister(ADXL345_REG_DEVID);if (deviceID != 0xE5) {Serial.println("Error: ADXL345 not found");while(1);}// 設置數據速率和帶寬為100HzwriteRegister(ADXL345_REG_BW_RATE, 0x0B);// 設置數據格式為±4g，全分辨率模式writeRegister(ADXL345_REG_DATA_FORMAT, 0x09);// 啟用測量模式writeRegister(ADXL345_REG_POWER_CTL, 0x08);Serial.println("ADXL345 initialized successfully");
}// 寫入寄存器函數
void writeRegister(byte reg, byte value) {Wire.beginTransmission(ADXL345_ADDRESS);Wire.write(reg);Wire.write(value);Wire.endTransmission();
}// 讀取寄存器函數
byte readRegister(byte reg) {Wire.beginTransmission(ADXL345_ADDRESS);Wire.write(reg);Wire.endTransmission();Wire.requestFrom(ADXL345_ADDRESS, 1);return Wire.read();
}// 讀取加速度數據
void readAcceleration(float &x, float &y, float &z) {Wire.beginTransmission(ADXL345_ADDRESS);Wire.write(ADXL345_REG_DATAX0);Wire.endTransmission();// 檢查是否有數據可用if (Wire.requestFrom(ADXL345_ADDRESS, 6) == 6) {// 讀取原始數據int16_t xRaw = (Wire.read() | (Wire.read() << 8));int16_t yRaw = (Wire.read() | (Wire.read() << 8));int16_t zRaw = (Wire.read() | (Wire.read() << 8));// 轉換為g值 (±4g范圍，全分辨率模式)x = xRaw * 0.0078;y = yRaw * 0.0078;z = zRaw * 0.0078;// 應用校準偏移if (calibrated) {x -= xOffset;y -= yOffset;z -= zOffset;}} else {// 讀取失敗，設置為0x = y = z = 0;Serial.println("Error: Failed to read acceleration data");}
}// 應用移動平均濾波
void applyMovingAverageFilter(float &x, float &y, float &z) {// 更新緩沖區movingAvgBufferX[movingAvgIndex] = x;movingAvgBufferY[movingAvgIndex] = y;movingAvgBufferZ[movingAvgIndex] = z;movingAvgIndex = (movingAvgIndex + 1) % MOVING_AVG_SIZE;// 計算平均值float xSum = 0, ySum = 0, zSum = 0;for (int i = 0; i < MOVING_AVG_SIZE; i++) {xSum += movingAvgBufferX[i];ySum += movingAvgBufferY[i];zSum += movingAvgBufferZ[i];}x = xSum / MOVING_AVG_SIZE;y = ySum / MOVING_AVG_SIZE;z = zSum / MOVING_AVG_SIZE;
}// 應用卡爾曼濾波
void applyKalmanFilter(float &x, float &y, float &z) {x = updateKalmanFilter(&kalmanX, x);y = updateKalmanFilter(&kalmanY, y);z = updateKalmanFilter(&kalmanZ, z);
}// 檢測穩定性
bool checkStability(float x, float y, float z, float prevX, float prevY, float prevZ) {float deltaX = abs(x - prevX);float deltaY = abs(y - prevY);float deltaZ = abs(z - prevZ);return (deltaX < STABILITY_THRESHOLD && deltaY < STABILITY_THRESHOLD && deltaZ < STABILITY_THRESHOLD);
}// 計算Roll和Pitch角度
void calculateAngles(float x, float y, float z, float &roll, float &pitch) {// 檢查數據有效性，避免除零錯誤和NaNif (isnan(x) || isnan(y) || isnan(z)) {roll = pitch = 0;return;}// 使用改進的公式計算Roll和Pitchfloat denominator = sqrt(x*x + z*z);if (denominator < 0.001) denominator = 0.001; // 避免除零錯誤roll = atan2(y, denominator) * 180.0 / PI;denominator = sqrt(y*y + z*z);if (denominator < 0.001) denominator = 0.001; // 避免除零錯誤pitch = atan2(-x, denominator) * 180.0 / PI;
}// 自動校準函數
void autoCalibrate() {Serial.println("Start automatic calibration...");Serial.println("Please ensure that the sensor is placed horizontally and stationary");// 等待2秒讓用戶放置傳感器delay(2000);float xSum = 0, ySum = 0, zSum = 0;const int samples = 200;int validSamples = 0;// 采集多個樣本求平均值for (int i = 0; i < samples; i++) {float x, y, z;readAcceleration(x, y, z);// 檢查數據有效性if (!isnan(x) && !isnan(y) && !isnan(z)) {xSum += x;ySum += y;zSum += z;validSamples++;}delay(10); // 每10ms采集一次}if (validSamples > 0) {// 計算偏移量xOffset = xSum / validSamples;yOffset = ySum / validSamples;zOffset = zSum / validSamples - 1.0; // 減去重力加速度// 設置硬件偏移寄存器writeRegister(ADXL345_REG_OFSX, (byte)(xOffset / 0.0156));writeRegister(ADXL345_REG_OFSY, (byte)(yOffset / 0.0156));writeRegister(ADXL345_REG_OFSZ, (byte)(zOffset / 0.0156));calibrated = true;Serial.println("Automatic calibration completed!");Serial.print("XOffset: "); Serial.print(xOffset, 6);Serial.print(" g, YOffset: "); Serial.print(yOffset, 6);Serial.print(" g, ZOffset: "); Serial.print(zOffset, 6);Serial.println(" g");} else {Serial.println("Calibration failure: No valid data can be obtained");}
}// 處理串口命令
void processSerialCommands() {if (Serial.available()) {char command = Serial.read();if (command == 'c' || command == 'C') {autoCalibrate();} else if (command == 'r' || command == 'R') {// 重置校準數據calibrated = false;xOffset = 0;yOffset = 0;zOffset = 0;writeRegister(ADXL345_REG_OFSX, 0);writeRegister(ADXL345_REG_OFSY, 0);writeRegister(ADXL345_REG_OFSZ, 0);Serial.println("The calibration data has been reset");} else if (command == 's' || command == 'S') {// 顯示狀態Serial.print("校準狀態 : ");Serial.println(calibrated ? "Calibrated Resistance" : "Uncalibrated");Serial.print("XOffset: "); Serial.println(xOffset, 6);Serial.print("YOffset: "); Serial.println(yOffset, 6);Serial.print("ZOffset: "); Serial.println(zOffset, 6);}}
}void setup() {Serial.begin(115200);// 初始化I2CWire.begin();// 初始化濾波緩沖區for (int i = 0; i < MOVING_AVG_SIZE; i++) {movingAvgBufferX[i] = 0;movingAvgBufferY[i] = 0;movingAvgBufferZ[i] = 0;}// 初始化卡爾曼濾波器initKalmanFilter(&kalmanX, KALMAN_Q, KALMAN_R, 0);initKalmanFilter(&kalmanY, KALMAN_Q, KALMAN_R, 0);initKalmanFilter(&kalmanZ, KALMAN_Q, KALMAN_R, 1.0); // Z軸初始值為1ginitADXL345();Serial.println("The ADXL345 attitude sensor is ready");Serial.println("Available commands:");Serial.println("c - START CALIBRATION");Serial.println("r - RESET the Calibration Data");Serial.println("s - DISPLAY STATUS");Serial.println("DATA FORMAT: Or: yaw pitch roll");Serial.println("          Accel: x y z");
}void loop() {// 處理串口命令processSerialCommands();static float prevX = 0, prevY = 0, prevZ = 0;float x, y, z;// 讀取加速度數據readAcceleration(x, y, z);// 檢查數據有效性if (isnan(x) || isnan(y) || isnan(z)) {Serial.println("Warning: Invalid acceleration data");delay(100);return;}// 應用移動平均濾波applyMovingAverageFilter(x, y, z);// 應用卡爾曼濾波applyKalmanFilter(x, y, z);// 檢測穩定性isStable = checkStability(x, y, z, prevX, prevY, prevZ);if (isStable) {lastStableTime = millis();}// 計算Roll和Pitch角度float roll, pitch;calculateAngles(x, y, z, roll, pitch);// 檢查角度數據有效性if (isnan(roll) || isnan(pitch)) {Serial.println("Warning: Invalid angle data");roll = pitch = 0;}// 輸出數據到ProcessingSerial.print("Or: ");Serial.print(0); // Yaw始終為0Serial.print(" ");Serial.print(pitch);Serial.print(" ");Serial.println(roll);Serial.print("Accel: ");Serial.print(x);Serial.print(" ");Serial.print(y);Serial.print(" ");Serial.println(z);prevX = x;prevY = y;prevZ = z;delay(20); // 50Hz更新率
}

三、校準過程及展示

3.1 校準過程

（1）將設備水平放置在穩定表面

打開操作窗口，按下鍵盤'C'鍵發送校準命令

（2）等待校準完成提示和偏移量

????????Sent calibration command
????????Raw: Start automatic calibration...
????????Raw: Please ensure that the sensor is placed horizontally and stationary

????????Raw: Automatic calibration completed!
????????Raw: XOffset: 0.438828 g, YOffset: 0.470731 g, ZOffset: 1.747395 g

（3）查看串口輸出校準后的加速度計數值

3.2 上位機界面展示

? ? ? ? Processing上位機端代碼運行后：

? ? ? ? 左側顯示三維立方體效果、右側展示數據面板

3.3 視頻演示

ADXL345加速度計進行Processing上位機展示

傾斜改變ADXL345傳感器的不同方向實時觀察上位機3D模型姿態變化

四、ADXL345寄存器工作原理

4.1?DATA_FORMAT寄存器(0x31)

????????控制數據的表示寄存器0x32到0x37，同時設置分辨率

4.2?BW_RATE寄存器(0x2C)

????????輸出數據速率為200HZ(BW_RATE寄存器中的LOW_POWER位（D4) = 0，速率位(D3:D0) = 0xB）

4.3?POWER_CTL寄存器(0x2D)

? ? ? ? 設置POWER_CTL寄存器為0x08測量模式

4.4 加速度計I2C通信

????????若將 CS 引腳接至 V DD I/O，ADXL345 便會進入 I2C 模式，此時僅需簡單的 2 線式連接即可。

????????當 ALT ADDRESS 引腳為高電平時，器件的 7 位 I2C 地址為 0x1D，后續緊跟 R/W 位，對應寫入地址為 0x3A，讀取地址為 0x3B。若把 ALT ADDRESS 引腳（即引腳 12）接地，則可選用備用 I2C 地址 0x53（后續同樣緊跟 R/W 位），相應的寫入地址為 0xA6，讀取地址為 0xA7。