跌倒检测代码分析_c# 跌倒识别-优快云博客

本文链接：https://blog.youkuaiyun.com/qwetyunk/article/details/148188395

首先，这是一个 C++ 类的实现文件（通常是 .cpp 文件），它包含了 FallDetection 类的方法（函数）的定义。

C++

#include "fall_detection.h"

#include "fall_detection.h": 这是一个预处理指令。它告诉编译器在编译这个文件之前，先包含 fall_detection.h 这个头文件的内容。头文件通常包含类的声明（class declaration）、函数原型（function prototypes）以及常量定义等，以便编译器知道这些元素的存在和类型。

C++

FallDetection::FallDetection() :
    DMPReady(false),
    devStatus(0),
    packetSize(0),
    isMonitoring(false),
    lastPitch(0),
    lastRoll(0),
    lastTime(0),
    fallenState(false),
    fallStartTime(0),
    lastFallAlert(0),
    stableStartTime(0),
    fallCallback(nullptr) {
}

FallDetection::FallDetection(): 这是 FallDetection 类的构造函数。当创建一个 FallDetection 类的对象时，这个函数会自动被调用。
: (初始化列表): 冒号后面是成员初始化列表。这是一种在对象创建时初始化其成员变量的推荐方式，效率通常高于在构造函数体内部赋值。
- DMPReady(false): 将布尔型成员变量 DMPReady 初始化为 false，表示 DMP（Digital Motion Processor，MPU6050 内部的一个处理器）尚未准备好。
- devStatus(0): 将整型成员变量 devStatus 初始化为 0，用于存储设备状态。
- packetSize(0): 将整型成员变量 packetSize 初始化为 0，用于存储 DMP FIFO 数据包大小。
- isMonitoring(false): 将布尔型成员变量 isMonitoring 初始化为 false，表示跌倒监测尚未启动。
- lastPitch(0), lastRoll(0): 初始化 lastPitch 和 lastRoll 为 0，用于存储上一次读取到的俯仰角和横滚角。
- lastTime(0): 初始化 lastTime 为 0，用于存储上一次更新时间。
- fallenState(false): 将布尔型成员变量 fallenState 初始化为 false，表示当前没有处于跌倒状态。
- fallStartTime(0): 初始化 fallStartTime 为 0，用于记录跌倒开始的时间。
- lastFallAlert(0): 初始化 lastFallAlert 为 0，用于记录上一次发送跌倒警报的时间。
- stableStartTime(0): 初始化 stableStartTime 为 0，用于记录姿态恢复稳定开始的时间。
- fallCallback(nullptr): 初始化 fallCallback 为 nullptr。这是一个函数指针，用于在检测到跌倒时调用外部函数，nullptr 表示它当前没有指向任何函数。
{}: 构造函数体，这里是空的，因为所有初始化都在初始化列表中完成了。

C++

bool FallDetection::begin() {
    Serial.println("初始化MPU6050...");

bool FallDetection::begin(): 这是 FallDetection 类的一个公共方法，名为 begin，返回一个布尔值（表示初始化是否成功）。这个方法通常用于执行设备的初始化设置。
Serial.println("初始化MPU6050...");: 在串口监视器上打印一条信息，指示 MPU6050 初始化过程开始。Serial.println 是 Arduino 环境中常用的串口输出函数。

C++

    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        Wire.setClock(400000);  // 400kHz I2C clock
        delay(100);  // 添加延时确保I2C总线稳定
    #endif

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE ... #endif: 这是一个预处理条件编译指令。它检查 I2CDEV_IMPLEMENTATION 这个宏是否定义为 I2CDEV_ARDUINO_WIRE。这通常用于兼容不同的 I2C 库实现。
- Wire.begin();: 初始化 Arduino 的 Wire 库（用于 I2C 通信）。
- Wire.setClock(400000);: 将 I2C 通信时钟设置为 400kHz (快速模式)。
- delay(100);: 暂停 100 毫秒，给 I2C 总线一些时间来稳定。

C++

    mpu.initialize();

mpu.initialize();: 调用 mpu 对象的 initialize 方法。mpu 应该是一个 MPU6050 类的实例，这个方法用于 MPU6050 传感器的基本初始化。

C++

    // 添加重试机制
    int retryCount = 0;
    const int maxRetries = 3;

    while (!mpu.testConnection() && retryCount < maxRetries) {
        Serial.printf("MPU6050连接失败，正在重试 (%d/%d)...\n", retryCount + 1, maxRetries);
        delay(1000);
        retryCount++;
    }

    if (retryCount >= maxRetries) {
        Serial.println("MPU6050连接失败，请检查接线和I2C地址!");
        return false;
    }

重试机制: 这段代码实现了一个简单的重试机制，以确保 MPU6050 传感器能够成功连接。
- int retryCount = 0;: 初始化重试计数器为 0。
- const int maxRetries = 3;: 定义最大重试次数为 3。
- while (!mpu.testConnection() && retryCount < maxRetries): 循环条件是 MPU6050 连接未成功 (!mpu.testConnection()) 并且重试次数未达到最大值。
  - mpu.testConnection(): 检查 MPU6050 传感器是否连接成功。
  - Serial.printf(...): 打印连接失败和重试进度的信息。printf 允许格式化输出。
  - delay(1000);: 每次重试前等待 1 秒。
  - retryCount++;: 增加重试计数。
- if (retryCount >= maxRetries): 如果循环结束时重试次数达到了最大值，说明连接失败。
  - Serial.println("MPU6050连接失败，请检查接线和I2C地址!");: 打印错误信息。
  - return false;: begin 方法返回 false，表示初始化失败。

C++

    Serial.println("MPU6050连接成功");

    devStatus = mpu.dmpInitialize();
    if (devStatus == 0) {
        Serial.println("开始校准MPU6050...");
        calibrate();
        mpu.setDMPEnabled(true);
        DMPReady = true;
        packetSize = mpu.dmpGetFIFOPacketSize();
        Serial.println("MPU6050初始化完成!");
        return true;
    }
    Serial.println("DMP初始化失败!");
    return false;
}

Serial.println("MPU6050连接成功");: 如果 MPU6050 连接成功，打印这条信息。
devStatus = mpu.dmpInitialize();: 调用 mpu 对象的 dmpInitialize 方法，初始化 MPU6050 内部的 DMP (Digital Motion Processor)。DMP 负责处理传感器原始数据，提供更高级的姿态数据（如四元数、欧拉角）。
if (devStatus == 0): 如果 dmpInitialize 返回 0，表示 DMP 初始化成功。
- Serial.println("开始校准MPU6050...");: 打印校准开始信息。
- calibrate();: 调用 FallDetection 类自己的 calibrate 方法对传感器进行校准。
- mpu.setDMPEnabled(true);: 启用 MPU6050 的 DMP。
- DMPReady = true;: 将 DMPReady 成员变量设置为 true，表示 DMP 已准备就绪。
- packetSize = mpu.dmpGetFIFOPacketSize();: 获取 DMP FIFO 缓冲区中每个数据包的大小。
- Serial.println("MPU6050初始化完成!");: 打印初始化完成信息。
- return true;: begin 方法返回 true，表示初始化成功。
Serial.println("DMP初始化失败!");: 如果 dmpInitialize 返回非 0 值，表示 DMP 初始化失败。
return false;: begin 方法返回 false，表示初始化失败。

C++

void FallDetection::calibrate() {
    mpu.CalibrateAccel(6);
    mpu.CalibrateGyro(6);
}

void FallDetection::calibrate(): 这是一个私有或保护方法，用于校准 MPU6050 传感器。
- mpu.CalibrateAccel(6);: 调用 mpu 对象的 CalibrateAccel 方法校准加速度计。参数 6 可能表示执行 6 次测量进行校准。
- mpu.CalibrateGyro(6);: 调用 mpu 对象的 CalibrateGyro 方法校准陀螺仪。

C++

void FallDetection::readSensorData() {
    if (mpu.dmpGetCurrentFIFOPacket(FIFOBuffer)) {
        mpu.dmpGetQuaternion(&q, FIFOBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    }
}

void FallDetection::readSensorData(): 这个方法用于从 MPU6050 读取传感器数据。
- if (mpu.dmpGetCurrentFIFOPacket(FIFOBuffer)): 尝试从 MPU6050 的 FIFO 缓冲区读取一个 DMP 数据包。如果成功读取，返回 true。FIFOBuffer 是一个字节数组，用于存储原始数据包。
  - mpu.dmpGetQuaternion(&q, FIFOBuffer);: 从 FIFO 数据包中解析出四元数（Quaternion）并存储到成员变量 q 中。四元数是表示姿态的一种方式。
  - mpu.dmpGetGravity(&gravity, &q);: 根据四元数计算出重力矢量，并存储到成员变量 gravity 中。
  - mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);: 根据四元数和重力矢量计算出偏航角（Yaw）、俯仰角（Pitch）和横滚角（Roll），并存储到成员数组 ypr 中。ypr[0] 通常是 Yaw，ypr[1] 是 Pitch，ypr[2] 是 Roll。

C++

void FallDetection::startMonitoring() {
    isMonitoring = true;
    Serial.println("跌倒监测已启动");
}

void FallDetection::startMonitoring(): 这个方法用于启动跌倒监测。
- isMonitoring = true;: 将 isMonitoring 成员变量设置为 true，表示监测已启动。
- Serial.println("跌倒监测已启动");: 打印启动信息。

C++

void FallDetection::stopMonitoring() {
    isMonitoring = false;
    Serial.println("跌倒监测已停止");
}

void FallDetection::stopMonitoring(): 这个方法用于停止跌倒监测。
- isMonitoring = false;: 将 isMonitoring 成员变量设置为 false，表示监测已停止。
- Serial.println("跌倒监测已停止");: 打印停止信息。

C++

// 修改：直接返回成员变量中缓存的数据
bool FallDetection::getCurrentData(Quaternion &outQ, VectorFloat &outGravity, float (&outYpr)[3]) {
    if (!DMPReady) return false;

    // 复制缓存的数据到输出参数
    outQ = q;
    outGravity = gravity;
    outYpr[0] = ypr[0];
    outYpr[1] = ypr[1];
    outYpr[2] = ypr[2];

    return true; // 假设数据总是有效的（如果 DMPReady）
}

bool FallDetection::getCurrentData(Quaternion &outQ, VectorFloat &outGravity, float (&outYpr)[3]): 这个方法允许外部获取当前缓存的传感器数据。它通过引用传递参数来“输出”数据。
- if (!DMPReady) return false;: 如果 DMP 未准备好，则无法获取数据，直接返回 false。
- outQ = q;: 将类内部缓存的四元数 q 复制给输出参数 outQ。
- outGravity = gravity;: 将类内部缓存的重力矢量 gravity 复制给输出参数 outGravity。
- outYpr[0] = ypr[0]; outYpr[1] = ypr[1]; outYpr[2] = ypr[2];: 将类内部缓存的 Yaw、Pitch、Roll 值复制给输出数组 outYpr。
- return true;: 如果 DMP 已准备好且数据已复制，返回 true。

C++

void FallDetection::update() {
    if (!DMPReady || !isMonitoring) return;
    static unsigned long lastPrintTime = 0;
    static unsigned long lastDataUpdateTime = 0;
    const unsigned long DATA_UPDATE_INTERVAL = 100;    // 数据更新间隔（毫秒）

void FallDetection::update(): 这是 FallDetection 类的核心方法，应该在一个循环中周期性地调用（例如 Arduino 的 loop() 函数），用于读取传感器数据、执行跌倒检测逻辑和更新状态。
- if (!DMPReady || !isMonitoring) return;: 如果 DMP 未准备好或监测未启动，则不执行任何操作，直接返回。
- static unsigned long lastPrintTime = 0;: 静态局部变量 lastPrintTime，用于控制串口打印的频率。static 意味着它只初始化一次，并且在函数调用之间保留其值。
- static unsigned long lastDataUpdateTime = 0;: 静态局部变量 lastDataUpdateTime，用于控制传感器数据更新的频率。
- const unsigned long DATA_UPDATE_INTERVAL = 100;: 定义数据更新的间隔为 100 毫秒。

C++

    // 从FIFO读取数据并存储到成员变量 q, gravity, ypr
    if (mpu.dmpGetCurrentFIFOPacket(FIFOBuffer)) {
        mpu.dmpGetQuaternion(&q, FIFOBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

        float currentPitch = ypr[1] * 180/M_PI;
        float currentRoll = ypr[2] * 180/M_PI;
        unsigned long currentTime = millis();

if (mpu.dmpGetCurrentFIFOPacket(FIFOBuffer)): 再次尝试从 MPU6050 的 FIFO 缓冲区读取一个 DMP 数据包。如果成功读取，执行以下代码。
- mpu.dmpGetQuaternion(&q, FIFOBuffer);, mpu.dmpGetGravity(&gravity, &q);, mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);: 这三行与 readSensorData 方法中的功能相同，从 FIFO 数据包中解析姿态数据并存储到成员变量中。
- float currentPitch = ypr[1] * 180/M_PI;: 将弧度制的俯仰角 ypr[1] 转换为度数，并存储到 currentPitch。M_PI 是圆周率 pi 的宏定义。
- float currentRoll = ypr[2] * 180/M_PI;: 将弧度制的横滚角 ypr[2] 转换为度数，并存储到 currentRoll。
- unsigned long currentTime = millis();: 获取当前距离 Arduino 程序启动以来的毫秒数。

C++

        // 定期更新和显示传感器数据
        if (currentTime - lastDataUpdateTime >= DATA_UPDATE_INTERVAL) {
            // Serial.printf("姿态数据 - Pitch: %.2f°, Roll: %.2f°\n", currentPitch, currentRoll); // 暂时注释掉过于频繁的打印
            lastDataUpdateTime = currentTime;
        }

定期更新和显示传感器数据:
- if (currentTime - lastDataUpdateTime >= DATA_UPDATE_INTERVAL): 检查距离上次数据更新/显示的时间是否超过了 DATA_UPDATE_INTERVAL (100 毫秒)。
  - // Serial.printf(...): 这行被注释掉了，表示在实际运行中可能不会频繁打印姿态数据。如果取消注释，它会打印当前的俯仰角和横滚角。
  - lastDataUpdateTime = currentTime;: 更新 lastDataUpdateTime 为当前时间。

C++

        // 状态输出
        if (currentTime - lastPrintTime >= 1000) { // 减少打印频率
            if (fallenState) {
                Serial.println("⚠️ 警告：老人处于倒地状态！");
                Serial.println("持续时间：" + String((currentTime - fallStartTime) / 1000) + "秒");
            } else {
                // Serial.println("✓ 老人状态正常"); // 正常状态不必频繁打印
            }
            lastPrintTime = currentTime;
        }

状态输出:
- if (currentTime - lastPrintTime >= 1000): 每秒钟（1000 毫秒）打印一次状态信息，以避免频繁刷屏。
  - if (fallenState): 如果 fallenState 为 true（表示已倒地）。
    - 打印警告信息和倒地持续时间。
  - else: 如果未倒地。
    - // Serial.println("✓ 老人状态正常");: 这行被注释掉了，表示正常状态下不频繁打印信息。
  - lastPrintTime = currentTime;: 更新 lastPrintTime。

C++

        if (lastTime != 0) {
            float deltaTime = (currentTime - lastTime) / 1000.0;
            // 避免 deltaTime 过小导致速率异常
            if (deltaTime < 0.01) deltaTime = 0.01;
            float pitchRate = abs(currentPitch - lastPitch) / deltaTime;
            float rollRate = abs(currentRoll - lastRoll) / deltaTime;

            bool isAbnormalAngle = (abs(currentPitch) > FALL_ANGLE_THRESHOLD ||
                                     abs(currentRoll) > FALL_ANGLE_THRESHOLD);
            bool isHighRate = (pitchRate > FALL_RATE_THRESHOLD ||
                               rollRate > FALL_RATE_THRESHOLD);
            bool isStableAngle = (abs(currentPitch) <= STABLE_ANGLE_THRESHOLD &&
                                   abs(currentRoll) <= STABLE_ANGLE_THRESHOLD);

跌倒检测核心逻辑开始:
- if (lastTime != 0): 确保 lastTime 已经有值（不是初始的 0），避免第一次运行时计算错误。
- float deltaTime = (currentTime - lastTime) / 1000.0;: 计算两次更新之间的时间间隔（秒）。
- if (deltaTime < 0.01) deltaTime = 0.01;: 避免 deltaTime 过小导致除以零或计算出极大速率值，将其限制在最小 0.01 秒。
- float pitchRate = abs(currentPitch - lastPitch) / deltaTime;: 计算俯仰角的变化速率（度/秒）。
- float rollRate = abs(currentRoll - lastRoll) / deltaTime;: 计算横滚角的变化速率（度/秒）。
- bool isAbnormalAngle = ...: 判断当前俯仰角或横滚角的绝对值是否超过了 FALL_ANGLE_THRESHOLD（跌倒角度阈值）。
- bool isHighRate = ...: 判断俯仰角变化速率或横滚角变化速率是否超过了 FALL_RATE_THRESHOLD（跌倒速率阈值）。
- bool isStableAngle = ...: 判断当前俯仰角和横滚角的绝对值是否都小于等于 STABLE_ANGLE_THRESHOLD（稳定角度阈值）。

C++

            // 跌倒检测逻辑
            if (!fallenState && isAbnormalAngle && isHighRate) {
                fallenState = true;
                fallStartTime = currentTime;
                stableStartTime = 0; // 重置稳定开始时间
                Serial.println("⚠️ 警告：检测到老人跌倒！");
                Serial.println("需要立即救助！");
                if (fallCallback) {
                    fallCallback("老人跌倒警报！");
                }
            }

初次跌倒检测:
- if (!fallenState && isAbnormalAngle && isHighRate): 如果当前未处于跌倒状态 (!fallenState)，并且同时满足“异常角度”和“高变化速率”两个条件，则认为发生了跌倒。
  - fallenState = true;: 将 fallenState 设置为 true。
  - fallStartTime = currentTime;: 记录跌倒开始的时间。
  - stableStartTime = 0;: 重置稳定开始时间。
  - 打印跌倒警告信息。
  - if (fallCallback) { fallCallback("老人跌倒警报！"); }: 如果 fallCallback 函数指针不为空，则调用它，传递跌倒警报信息。这允许外部模块响应跌倒事件。

C++

            // 持续倒地状态与恢复逻辑
            else if (fallenState) {
                if (!isStableAngle) { // 如果姿态不稳（仍处于倾斜）
                    stableStartTime = 0; // 重置稳定开始时间
                    if (currentTime - lastFallAlert > ALERT_INTERVAL) {
                        if (fallCallback) {
                            fallCallback("老人持续处于倒地状态，请立即查看！");
                        }
                        Serial.println("⚠️ 严重警告：老人仍未得到救助！");
                        Serial.println("倒地持续时间：" + String((currentTime - fallStartTime) / 1000) + "秒");
                        lastFallAlert = currentTime;
                    }
                } else { // 如果姿态稳定（在 STABLE_ANGLE_THRESHOLD 内）
                    if (stableStartTime == 0) { // 刚开始稳定
                        stableStartTime = currentTime;
                        Serial.println("检测到姿态恢复稳定，开始计时...");
                    } else { // 已经稳定了一段时间
                        if (currentTime - stableStartTime > RECOVERY_DURATION_THRESHOLD) { // 稳定时间足够长
                            fallenState = false;
                            stableStartTime = 0; // 重置
                            fallStartTime = 0; // 重置
                            if (fallCallback) {
                                fallCallback("老人已恢复正常姿态");
                            }
                            Serial.println("✓ 老人已恢复正常姿态");
                        }
                    }
                }
            }

持续倒地与恢复逻辑: 当已经处于 fallenState (倒地状态) 时，执行这段逻辑。
- if (!isStableAngle): 如果当前姿态仍然不稳定（即仍倾斜，未达到稳定角度阈值）。
  - stableStartTime = 0;: 重置 stableStartTime，因为姿态不稳，不能认为在恢复中。
  - if (currentTime - lastFallAlert > ALERT_INTERVAL): 如果距离上次发送持续倒地警报的时间超过了 ALERT_INTERVAL。
    - if (fallCallback) { fallCallback("老人持续处于倒地状态，请立即查看！"); }: 调用回调函数发送持续倒地警报。
    - 打印严重警告信息和倒地持续时间。
    - lastFallAlert = currentTime;: 更新 lastFallAlert 时间。
- else { ... }: 如果当前姿态稳定（即在 STABLE_ANGLE_THRESHOLD 范围内）。
  - if (stableStartTime == 0): 如果 stableStartTime 是 0，说明这是姿态刚开始稳定。
    - stableStartTime = currentTime;: 记录姿态开始稳定的时间。
    - 打印信息表示开始计时。
  - else: 如果 stableStartTime 不为 0，说明姿态已经稳定了一段时间。
    - if (currentTime - stableStartTime > RECOVERY_DURATION_THRESHOLD): 检查姿态稳定持续的时间是否超过了 RECOVERY_DURATION_THRESHOLD（恢复所需持续时间阈值）。
      - fallenState = false;: 如果稳定时间足够长，认为老人已恢复，将 fallenState 设置为 false。
      - stableStartTime = 0;, fallStartTime = 0;: 重置相关计时器。
      - if (fallCallback) { fallCallback("老人已恢复正常姿态"); }: 调用回调函数发送恢复信息。
      - 打印恢复正常姿态的信息。

C++

        }

        lastPitch = currentPitch;
        lastRoll = currentRoll;
        lastTime = currentTime;
    }
}

更新历史数据:
- lastPitch = currentPitch;: 将当前俯仰角保存为 lastPitch，用于下一次循环计算速率。
- lastRoll = currentRoll;: 将当前横滚角保存为 lastRoll。
- lastTime = currentTime;: 将当前时间保存为 lastTime。
}: 结束 if (mpu.dmpGetCurrentFIFOPacket(FIFOBuffer)) 块。
}: 结束 update() 方法。

总的来说，这段代码实现了以下功能：

MPU6050 传感器初始化：包括 I2C 通信设置、传感器连接测试（带重试机制）和 DMP 初始化及校准。
跌倒监测控制：startMonitoring() 和 stopMonitoring() 方法用于开启和关闭监测功能。
传感器数据读取：周期性地从 MPU6050 的 DMP 中读取四元数、重力矢量和欧拉角（Yaw, Pitch, Roll）。
跌倒检测算法：
- 基于当前姿态角度（Pitch/Roll）和姿态变化速率（Pitch Rate/Roll Rate）来判断是否发生跌倒。
- 定义了 FALL_ANGLE_THRESHOLD 和 FALL_RATE_THRESHOLD 作为跌倒的判断条件。
跌倒状态管理与恢复判断：
- 一旦检测到跌倒，进入 fallenState。
- 在跌倒状态下，如果姿态持续不稳定，会定期发送警报。
- 如果姿态恢复稳定并持续一定时间（RECOVERY_DURATION_THRESHOLD），则认为老人已恢复正常，退出跌倒状态。
回调机制：通过 fallCallback 函数指针，允许在检测到跌倒或恢复时通知外部程序，实现警报联动功能。
串口输出：通过 Serial.println 和 Serial.printf 在串口监视器上实时输出状态和警报信息。