mirror of
https://github.com/espressif/esp-drone.git
synced 2026-07-21 10:15:36 +00:00
build docs with sphinx
This commit is contained in:
libo
+5
-1
@@ -3,4 +3,8 @@ sdkconfig
|
||||
sdkconfig.old
|
||||
esp_idf
|
||||
.vscode/
|
||||
|
||||
docs/zh_CN/md/_build
|
||||
docs/en/md/_build
|
||||
docs/zh_CN/md/sphinx-warning-log.txt
|
||||
docs/__pycache__
|
||||
docs/en/md/sphinx-warning-log.txt
|
||||
|
||||
@@ -0,0 +1,39 @@
|
||||
stages:
|
||||
- build
|
||||
|
||||
variables:
|
||||
BATCH_BUILD: "1"
|
||||
V: "0"
|
||||
MAKEFLAGS: "-j5 --no-keep-going"
|
||||
IDF_PATH: "$CI_PROJECT_DIR/esp-idf"
|
||||
IDF_CI_BUILD: "1"
|
||||
|
||||
build_docs:
|
||||
stage: build
|
||||
image: $CI_DOCKER_REGISTRY/esp32-ci-env$BOT_DOCKER_IMAGE_TAG
|
||||
tags:
|
||||
- build_docs
|
||||
artifacts:
|
||||
when: always
|
||||
paths:
|
||||
# English version of documentation
|
||||
# - docs/en/md/sphinx-warning-log.txt
|
||||
# - docs/en/md/_build/html
|
||||
# Chinese version of documentation
|
||||
- docs/zh_CN/md/sphinx-warning-log.txt
|
||||
- docs/zh_CN/md/_build/html
|
||||
expire_in: 1 week
|
||||
before_script:
|
||||
- echo "skip default before_script"
|
||||
script:
|
||||
- cd docs
|
||||
# make English version of documentation
|
||||
# - cd en/
|
||||
# - cd md/
|
||||
# - make clean && make html
|
||||
# - cd ../
|
||||
# make Chinese version of documentation
|
||||
- cd zh_CN/
|
||||
- cd md/
|
||||
- make clean && make html
|
||||
- cd ../
|
||||
@@ -0,0 +1,17 @@
|
||||
# .readthedocs.yml
|
||||
# Read the Docs configuration file
|
||||
# See https://docs.readthedocs.io/en/stable/config-file/v2.html for details
|
||||
|
||||
# Required
|
||||
version: 2
|
||||
|
||||
# Optionally build your docs in additional formats such as PDF and ePub
|
||||
formats:
|
||||
- htmlzip
|
||||
- pdf
|
||||
|
||||
# Optionally set the version of Python and requirements required to build your docs
|
||||
python:
|
||||
version: 2.7
|
||||
install:
|
||||
- requirements: docs/requirements.txt
|
||||
@@ -5,15 +5,15 @@
|
||||
|
||||
### Introduction
|
||||
|
||||
**ESP-Drone** is an open source **drone solution** based on espressif **ESP32-S2 / ESP32** Wi-Fi chip, which can be controlled through **Wi-Fi** network using mobile APP or gamepad. ESP-Drone supports multiple fly modes, `stabilize`, `height-hold`, `position-hold` and more. ESP-Drone solution has **simple hardware structure**,**clear and extendible code architecture**, can be used in **STEAM education** and other fields. The main code ported from **Crazyflie** open source project, using the **GPL3.0** open source protocol.
|
||||
**ESP-Drone** is an open source **drone solution** based on espressif **ESP32-S2 / ESP32** Wi-Fi chip, which can be controlled through **Wi-Fi** network using mobile APP or gamepad. ESP-Drone supports multiple flight modes, `stabilize`, `height-hold`, `position-hold` and more. With **simple hardware**, **clear and extendible code architecture**, ESP-Drone can be used in **STEAM education** and other fields. The main code ported from **Crazyflie** open source project with **GPL3.0** protocol.
|
||||
|
||||
**Hardware**：[Hardware](./docs/zh_CN/md/hardware.md)
|
||||
|
||||
**Getting Started**: [Getting Started](./docs/zh_CN/md/0_gettingstarted.md)
|
||||
**Getting Started**: [Getting Started](./docs/zh_CN/md/gettingstarted.md)
|
||||
|
||||
|
||||
|
||||
### Implemented Features
|
||||
### Features
|
||||
|
||||
1. Stabilize mode
|
||||
2. Height-hold mode
|
||||
|
||||
+4
-4
@@ -5,15 +5,15 @@
|
||||
|
||||
### 简介
|
||||
|
||||
ESP-Drone 是基于乐鑫 `ESP32-S2 / ESP32` 开发的小型无人机解决方案，可使用手机 APP 或游戏手柄通过 Wi-Fi 网络进行连接和控制，目前已支持自稳定飞行、定高飞行、定点飞行等多种模式。该方案硬件结构简单，代码架构清晰完善,方便功能扩展，可用于STEAM教育等领域。控制系统代码来自 Crazyflie 开源工程，使用GPL3.0开源协议。
|
||||
ESP-Drone 是基于乐鑫 `ESP32-S2 / ESP32` 开发的小型无人机解决方案，可使用手机 APP 或游戏手柄通过 Wi-Fi 网络进行连接和控制，目前已支持自稳定飞行、定高飞行、定点飞行等多种模式。该方案硬件结构简单，代码架构清晰完善，方便功能扩展，可用于STEAM教育等领域。控制系统代码来自 Crazyflie 开源工程，使用GPL3.0开源协议。
|
||||
|
||||
**硬件原理图**：[Hardware](./docs/zh_CN/md/hardware.md)
|
||||
|
||||
**从这里开始**: [Getting Started](./docs/zh_CN/md/0_gettingstarted.md)
|
||||
**从这里开始**: [Getting Started](./docs/zh_CN/md/gettingstarted.md)
|
||||
|
||||
|
||||
|
||||
### 已实现功能
|
||||
### 功能
|
||||
|
||||
1. 自稳定模式
|
||||
2. 定高模式
|
||||
@@ -33,7 +33,7 @@ Additional third party copyrighted code is included under the following licenses
|
||||
| core/crazyflie | GPL-3.0 |[Crazyflie](https://github.com/bitcraze/crazyflie-firmware) |a2a26abd53a5f328374877bfbcb7b25ed38d8111|
|
||||
| lib/dsp_lib | | [esp32-lin](https://github.com/whyengineer/esp32-lin/tree/master/components/dsp_lib) |6fa39f4cd5f7782b3a2a052767f0fb06be2378ff|
|
||||
|
||||
### 感谢/THANKS
|
||||
### 致谢
|
||||
|
||||
1. 感谢 Bitcraze 开源组织提供很棒的 [Crazyflie](https://www.bitcraze.io/%20)无人机项目代码
|
||||
2. 感谢 Espressif 提供 ESP32 和 [ESP-IDF操作系统](https://docs.espressif.com/projects/esp-idf/en/latest/esp32s2/get-started/index.html)
|
||||
|
||||
Vendored
BIN
Binary file not shown.
|
After Width: | Height: | Size: 6.8 KiB |
Vendored
BIN
Binary file not shown.
|
After Width: | Height: | Size: 6.5 KiB |
Vendored
BIN
Binary file not shown.
|
After Width: | Height: | Size: 6.4 KiB |
Vendored
|
Before Width: | Height: | Size: 188 KiB After Width: | Height: | Size: 188 KiB |
+1
-1
@@ -202,7 +202,7 @@ html_logo = "../../_static/espressif-logo.svg"
|
||||
# Add any paths that contain custom static files (such as style sheets) here,
|
||||
# relative to this directory. They are copied after the builtin static files,
|
||||
# so a file named "default.css" will overwrite the builtin "default.css".
|
||||
html_static_path = ['../../_static']
|
||||
html_static_path = ['../../_static','../../../hardware/']
|
||||
|
||||
# Add any extra paths that contain custom files (such as robots.txt or
|
||||
# .htaccess) here, relative to this directory. These files are copied
|
||||
|
||||
@@ -1,40 +0,0 @@
|
||||
|
||||
## ESP-Drone
|
||||
|
||||
**Drones powered by ESP32&ESP-IDF&Crazyflie**
|
||||
|
||||
### Introduction
|
||||
|
||||
**ESP-Drone** is an open source **drone solution** based on espressif **ESP32-S2 / ESP32** Wi-Fi chip, which can be controlled through **Wi-Fi** network using mobile APP or gamepad. ESP-Drone supports multiple fly modes, `stabilize`, `height-hold`, `position-hold` and more. ESP-Drone solution has **simple hardware structure**,**clear and extendible code architecture**, can be used in **STEAM education** and other fields. The main code ported from **Crazyflie** open source project, using the **GPL3.0** open source protocol.
|
||||
|
||||
**Hardware**：[Hardware](./docs/zh_CN/md/hardware.md)
|
||||
|
||||
**Getting Started**: [Getting Started](./docs/zh_CN/md/0_gettingstarted.md)
|
||||
|
||||
|
||||
|
||||
### Implemented Features
|
||||
|
||||
1. Stabilize mode
|
||||
2. Height-hold mode
|
||||
3. position-hold mode
|
||||
4. APP control
|
||||
5. cfclient supported
|
||||
|
||||
> 2/3:expansion module requried
|
||||
|
||||
### Third Party Copyrighted Code
|
||||
|
||||
Additional third party copyrighted code is included under the following licenses.
|
||||
|
||||
| Component | License | Origin |commit id |
|
||||
| :---: | :---: | :---: |:---: |
|
||||
| core/crazyflie | GPL-3.0 |[Crazyflie](https://github.com/bitcraze/crazyflie-firmware) |a2a26abd53a5f328374877bfbcb7b25ed38d8111|
|
||||
| lib/dsp_lib | | [esp32-lin](https://github.com/whyengineer/esp32-lin/tree/master/components/dsp_lib) |6fa39f4cd5f7782b3a2a052767f0fb06be2378ff|
|
||||
|
||||
### THANKS
|
||||
|
||||
1. Thanks to the Bitcraze for the great [Crazyflie project](https://www.bitcraze.io/%20)
|
||||
2. Thanks to Espressif for the powerful [ESP-IDF environment](https://docs.espressif.com/projects/esp-idf/en/latest/esp32s2/get-started/index.html)
|
||||
3. Thanks to WhyEngineer for the useful [ESP-DSP lib](https://github.com/whyengineer/esp32-lin/tree/master/components/dsp_lib)
|
||||
|
||||
@@ -1,63 +0,0 @@
|
||||
|
||||
## 项目预览
|
||||
|
||||
ESP-Drone 是基于乐鑫 ESP32/ESP32-S2 开发的小型无人机解决方案，可使用手机 APP 或游戏手柄通过 Wi-Fi 网络进行连接和控制，目前已支持自稳定飞行、定高飞行、定点飞行等多种模式。该方案硬件结构简单，代码架构清晰，支持功能扩展，可用于 STEAM 教育等领域。项目部分代码来自 Crazyflie 开源工程，继承 GPL3.0 开源协议。
|
||||
|
||||
|
||||
|
||||
### 功能介绍
|
||||
|
||||
* 支持自稳定模式-stabilize：自动控制机身水平，保持平稳飞行
|
||||
* 支持定高模式-heighthold：自动控制油门输出，保持固定高度
|
||||
* 支持定点模式-positionhold：自动控制机身角度，保持固定空间位置
|
||||
* 支持 PC 上位机调试：使用 cfclient 上位机进行静态/动态调试
|
||||
* 支持 APP 控制：使用手机 APP 通过 Wi-Fi 轻松控制
|
||||
* 支持游戏手柄控制-gamepad：通过 cfclient 使用游戏手柄轻松控制
|
||||
|
||||
### 硬件组成结构
|
||||
|
||||
**ESP-Drone 2.0 使用模块化的设计思路，由主控板和扩展板组成：**
|
||||
|
||||
* **主控制板**：搭载支持基础飞行的必要元器件，提供硬件扩展接口
|
||||
* **扩展板**：基于硬件扩展接口，用于支持高级飞行功能或其他 DIY 功能
|
||||
|
||||
|序号| 模块名 | 主要元器件 | 功能 | 接口 |安装位置 |
|
||||
|--|--|--|--|--|--|
|
||||
|1| **主控制板-ESP32-S2** | ESP32-S2-WROVER + MPU6050| 基础飞行 |提供 I2C SPI GPIO 扩展接口 ||
|
||||
|2|扩展板- **定点模块** | PMW3901 + VL53L1X | 室内定点飞行 | SPI + I2C | 底部，面向地面 |
|
||||
|3| 扩展板-**气压定高模块** | MS5611 气压 | 气压定高 | I2C 或 MPU6050从机|顶部或底部 |
|
||||
|4| 扩展板-指南针模块 | HMC5883 罗盘 | 无头模式等高级模式 | I2C 或 MPU6050从机|顶部或底部 |
|
||||
|
||||
详情可查阅：[hardware](./hardware.md)
|
||||
|
||||
|
||||
### ESP-IDF 简介
|
||||
|
||||
ESP-IDF 是乐鑫为 ESP32/ESP32-S2 提供的物联网开发框架。
|
||||
|
||||
* ESP-IDF 包含一系列库及头文件，提供了基于 ESP32/ESP32-S2 构建软件项目所需的核心组件。
|
||||
* ESP-IDF 还提供了开发和量产过程中最常用的工具及功能，例如：构建、烧录、调试和测量等。
|
||||
|
||||
详情可查阅：[ESP-IDF Get Started](https://docs.espressif.com/projects/esp-idf/en/latest/esp32s2/get-started/index.html)
|
||||
|
||||
### Crazyflie 简介
|
||||
|
||||
Crazyflie 是来自 bitcraze 开源工程的四旋翼飞行器。
|
||||
|
||||
* Crazyflie 支持多种传感器组合，可以轻松实现定高模式、定点模式等高级飞行模式。
|
||||
* Crazyflie 基于 FreeRTOS 编写，将复杂的无人机系统，分解成多个具有不同优先级的软件任务。
|
||||
* Crazyflie 设计了功能完备的 cfclient 上位机和 CRTP 通信协议，便于实现调试、测量和控制。
|
||||
|
||||
|
||||
|
||||
>图片来源：A swarm of drones exploring the environment, avoiding obstacles and each other. \(Guus Schoonewille, TU Delft\)
|
||||
|
||||
详情可查阅 crazyflie 官网：https://www.bitcraze.io/
|
||||
|
||||
## 使用指引
|
||||
|
||||
**请查阅**: [ESP-Drone User Guide](./1_user_guide.md)
|
||||
|
||||
## 开发指引
|
||||
|
||||
**请查阅**: [ESP-Drone Develop Guide](./2_developer_guide.md)
|
||||
@@ -1,138 +0,0 @@
|
||||
|
||||
## ESP-Drone_S2_V1.2 硬件组装过程
|
||||
|
||||
详细的硬件介绍可查阅：[hardware](./hardware.md)
|
||||
|
||||
|
||||
|
||||
TODO: 添加组装视频链接
|
||||
|
||||
## 手机 APP 使用指引
|
||||
|
||||
1. 安装ESPDroe APP
|
||||
|
||||
**Android APP 下载：**
|
||||
|
||||
[https://github.com/qljz1993/DailyMD_public/raw/master/esplanes2_test/ESP-Drone-android-debug.apk](https://github.com/qljz1993/DailyMD_public/raw/master/esplanes2_test/ESP-Drone-android-debug.apk)
|
||||
|
||||
**IOS APP 扫码下载：**
|
||||
|
||||
|
||||
|
||||
2. WiFi 连接
|
||||
|
||||
SSID：ESPDRONE_XXXX (XXXX根据 MAC 设置） PASSWORD：12345678
|
||||
|
||||
3. 个性化设置
|
||||
|
||||
```
|
||||
默认配置：
|
||||
|
||||
Flight control settings
|
||||
1. Mode: Mode2
|
||||
2. Deadzone: 0.2
|
||||
3. Roll trim: 0.0
|
||||
4. Pitch trim: 0.0
|
||||
5. Advanced flight control : true
|
||||
6. Advanced flight control preferences
|
||||
1. max roll/pitch angle: 15
|
||||
2. max yaw angle: 90
|
||||
3. max thrust: 90
|
||||
4. min thrust: 25
|
||||
5. X-Mode: true
|
||||
Controller settings
|
||||
1. use full travel for thrust: false
|
||||
2. virtual joystick size: 100
|
||||
App settings
|
||||
1. Screen rotation lock: true
|
||||
2. full screen mode:true
|
||||
3. show console: true
|
||||
```
|
||||
|
||||
4. 控制飞行
|
||||
1. 打开通信连接，小飞机绿灯将闪烁
|
||||
2. 轻推油门，起飞
|
||||
|
||||
|
||||
|
||||
## PC cfclient 使用指引
|
||||
|
||||
|
||||
|
||||
**1.安装crtp协议支持包**
|
||||
|
||||
1.1 下载源代码
|
||||
|
||||
```text
|
||||
git clone -b dev_esplane https://github.com/qljz1993/crazyflie-lib-python.git
|
||||
```
|
||||
|
||||
1.2 进入源码目录，安装依赖
|
||||
|
||||
```text
|
||||
pip3 install -r requirements.txt
|
||||
```
|
||||
|
||||
1.3 安装crtp包
|
||||
|
||||
```text
|
||||
pip3 install -e .
|
||||
```
|
||||
|
||||
**2.安装cfclient**
|
||||
|
||||
2.1 下载源代码
|
||||
|
||||
```text
|
||||
git clone -b dev_esplane https://github.com/qljz1993/crazyflie-clients-python.git
|
||||
```
|
||||
|
||||
2.2 进入源码目录，安装依赖
|
||||
|
||||
```text
|
||||
sudo apt-get install python3 python3-pip python3-pyqt5 python3-pyqt5.qtsvg
|
||||
```
|
||||
|
||||
2.3 安装cfclient客户端
|
||||
|
||||
```text
|
||||
pip3 install -e .
|
||||
```
|
||||
|
||||
2.4 启动客户端
|
||||
|
||||
|
||||
```text
|
||||
python3 ./bin/cfclient
|
||||
```
|
||||
|
||||
**3.配置遥控器**
|
||||
|
||||
|
||||
|
||||
3.1 配置4个控制轴 `Roll 、Pitch、Yaw、Thrust`
|
||||
|
||||
3.2 配置一个按键为`Assisted control` ，用于飞行模式切换
|
||||
|
||||
## 螺旋桨方向
|
||||
|
||||
* 注意按照下图所示位置，安装 A B 螺旋桨
|
||||
* 注意检查飞机上电自检时，螺旋桨转向是否正确？
|
||||
|
||||
|
||||
|
||||
## 其他注意事项
|
||||
|
||||
* 将飞机头部朝前放置，尾部天线朝向自己
|
||||
* 注意将飞机置于水平面上，机身稳定时上电
|
||||
* 注意观察上位机水平面是否置平？
|
||||
* 注意观察通信建立以后，小飞机尾部绿灯是否快速闪速
|
||||
* 注意观察小飞机头部红灯是否熄灭，亮起代表电量不足
|
||||
* 轻推左手小油门，检查飞机是否能快速响应
|
||||
* 轻推右手方向，检查方向控制是否正确
|
||||
* 起飞吧！
|
||||
|
||||
|
||||
## 开发指引
|
||||
|
||||
**请查阅**: [ESP-Drone Develop Guide](./2_developer_guide.md)
|
||||
@@ -1,33 +0,0 @@
|
||||
|
||||
## 软件开发指引
|
||||
|
||||
* [搭建开发环境](./get_espidf.md)
|
||||
* [ESP-IDF 开发环境搭建](./get_espidf.md#esp-idf-开发环境搭建)
|
||||
* [ESP32(S2) 链接脚本修改](./get_espidf.md#esp32s2-链接脚本修改)
|
||||
* [获取源代码](./get_sourcecode.md)
|
||||
* [驱动程序](./drivers.md)
|
||||
* [通用设备](./drivers.md#general)
|
||||
* [I2C 总线设备](./drivers.md#i2c_devices)
|
||||
* [SPI 总线设备](./drivers.md#spi_devices)
|
||||
* [飞控系统](./system.md)
|
||||
* [系统启动流程](./system.md#系统启动流程)
|
||||
* [系统任务管理](./system.md#系统-task-管理)
|
||||
* [关键任务介绍](./system.md#关键-task-介绍)
|
||||
* [传感器硬件抽象](./sensors.md#传感器硬件抽象层)
|
||||
* [传感器校准](./sensors.md#传感器校准过程)
|
||||
* [姿态计算](./estimator.md)
|
||||
* [控制算法](./controller.md)
|
||||
* [通信协议](./communication.md)
|
||||
* [通信层级结构](./communication.md#通信层级结构)
|
||||
* [Wi-Fi 通信](./communication.md#wi-fi-通信)
|
||||
* [UDP 通信](./communication.md#udp-通信)
|
||||
* [CRTP 协议](./communication.md#crtp-协议)
|
||||
* [基于 CRTP 协议的应用开发](./communication.md#基于-crtp-协议的应用开发)
|
||||
|
||||
## 硬件开发指引
|
||||
|
||||
* [硬件设计](./hardware.md)
|
||||
* [已支持硬件列表](./hardware.md#已支持硬件)
|
||||
* [ESP32_S2_Drone_V1_2](./hardware.md#esp32_s2_drone_v1_2)
|
||||
* [ESPlane_V2_S2](./hardware.md#esplane_v2_s2)
|
||||
* [ESPlane FC V1](./hardware.md#esplane-fc-v1)
|
||||
@@ -1,75 +0,0 @@
|
||||
|
||||
## CFclient 简介
|
||||
|
||||
CFclient 是 `crazeflie` 源工程的上位机，完全实现了 `CRTP` 协议中定义的功能，可以加快飞机的调试过程，ESP-Drone 项目对该上位机进行裁剪和调整，满足功能设计需求。
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
项目中有很多相关的文件，例如配置文件和缓存文件，其中JSON文件用来存储配置信息。关于配置信息中内容的解读：[User configuration file](https://www.bitcraze.io/docs/crazyflie-clients-python/master/dev_info_client/)
|
||||
|
||||
**基本飞行设置 Basic Flight Control**
|
||||
|
||||
1. 飞行模式 Flight mode selector \(Normal and Advanced\)
|
||||
* 基本模式 _Normal: 初学者使用
|
||||
* 高级模式 _Advanced: 设置解锁最大角度，设置最大油门。
|
||||
2. 自动模式 Assisted mode selection.
|
||||
* 定海拔模式 _Altitude hold_: 保持飞行海拔，需要气压计支持。
|
||||
* 定点模式 _Position hold_: 保持当前位置，需要光流和TOF支持。
|
||||
* 定高模式 _Height hold_: 保持相对高度, 触发时保持高于地面 40cm ，需要TOF支持。
|
||||
* 悬停模式 _Hover : 触发时保持高于地面 40cm，并悬停在起飞点，需要光流和TOF支持。
|
||||
3. 角度修正 trim
|
||||
* Roll Trim : 翻滚角修正，用于弥补传感器水平安装误差
|
||||
* Pitch Trim : 俯仰角修正，用于弥补传感器水平安装误差
|
||||
|
||||
> 在自动模式下，油门摇杆变为高度控制摇杆
|
||||
|
||||
**高级飞行设置 Advanced Flight Control**
|
||||
|
||||
1. 最大倾角 _Max angle: 设置最大允许的俯仰和翻滚角度 roll/pitch
|
||||
2. 最大自选速度 _Max yaw rate: 设置允许的偏航速度 yaw
|
||||
3. 最大油门 _Max thrust: 设置最大油门
|
||||
4. 最小油门 _Min thrust: 设置最小油门
|
||||
5. 回转极限 _Slew limit: 回转极限。防止油门骤降，油门低于该值时，将被平滑的接管。 Set the percentage where the thrust is slew controlled \(the thrust value lowering will be limited\). This makes the Crazyflie a bit easier to fly for beginners
|
||||
6. 回转率 _Slew rate: 油门到回转极限时，最大的油门值。When the thrust is below the slew limit, this is the maximum rate of lowering the thrust
|
||||
|
||||
**遥控器设置 Configure input device**
|
||||
|
||||
按照提示绑定遥控器摇杆与各个控制通道：
|
||||
|
||||
|
||||
|
||||
**飞行数据 Fligt Data**
|
||||
|
||||
驾驶仪可以看到当前飞机姿态，右下方显示对应的详细数据。
|
||||
1. 目标角度 Target
|
||||
2. 测量角度 Actual
|
||||
3. 当前油门值 Thrust
|
||||
4. 电机实际输出 M1\M2\M3\M4
|
||||
|
||||
## 在线参数修改 parameter
|
||||
|
||||
**注意事项**
|
||||
|
||||
1. 修改的参数实时生效，避免了频繁烧录固件。
|
||||
2. 可在代码中通过宏定义配置那些参数可被上位机实时修改。
|
||||
3. 注意参数在线修改仅用于调试，掉电不保存。
|
||||
|
||||
**在线调整 PID 参数**
|
||||
|
||||
|
||||
|
||||
## 飞行数据监控 logging
|
||||
|
||||
**配置要监控的参数**
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
**实时波形绘制**
|
||||
|
||||
陀螺仪加速度计实时数据监测：
|
||||
|
||||
|
||||
@@ -1,4 +1,6 @@
|
||||
|
||||
# 通信协议
|
||||
|
||||
## 通信层级结构
|
||||
|
||||
||||
|
||||
@@ -69,13 +71,13 @@ TODO: ESP32-S2 Wi-Fi 参数需要验证和补充
|
||||
|
||||
## UDP 通信
|
||||
|
||||
**UDP 端口号：**
|
||||
### UDP 端口号
|
||||
|
||||
| App |Direction | ESP-Drone |
|
||||
|--|--|--|--|
|
||||
| 192.168.43.42::2399 | TX / RX | 192.168.43.42::2390 |
|
||||
|
||||
**UDP 包结构：**
|
||||
### UDP 包结构
|
||||
|
||||
```text
|
||||
/* Frame format:
|
||||
@@ -106,7 +108,7 @@ ESP-Drone 项目继承 Crazyflie 项目使用的 CRTP \(Crazy RealTime Protocol\
|
||||
|
||||
> CRTP is designed to be state-less, so there's no handshaking procedure that is needed. Any command can be sent at any time, but for some logging/param/mem commands the TOC \(table of contents\) needs to be downloaded in order for the host to be able to send the correct information. The implementation of the Pyton API will download the param/log/mem TOC at connect in order to be able to use all the functionality.
|
||||
|
||||
**CRTP 包结构:**
|
||||
### CRTP 包结构
|
||||
|
||||
> Each CRTP packet is 32 bytes, of which 1 byte is taken by the header. This gives a total payload of 31 bytes per packet. The header holds the port \(8 bits\), channel \(2 bits\) and reserved \(2 bits\).
|
||||
|
||||
@@ -130,7 +132,7 @@ ESP-Drone 项目继承 Crazyflie 项目使用的 CRTP \(Crazy RealTime Protocol\
|
||||
| | 0 | 4-7 | The destination port |
|
||||
| Data | 1-31 | The data in the packet | |
|
||||
|
||||
**端口分配:**
|
||||
### 端口分配
|
||||
|
||||
| Port | Target | Used for |
|
||||
| :---: | :---: | :---: |
|
||||
@@ -149,7 +151,7 @@ ESP-Drone 项目继承 Crazyflie 项目使用的 CRTP \(Crazy RealTime Protocol\
|
||||
|
||||
* [各个端口使用详情可参考：crtp](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/)
|
||||
|
||||
**CRTP 协议支持包**
|
||||
### CRTP 协议支持包
|
||||
|
||||
* **cflib 是 CRTP 协议的 python 支持包，提供了通信协议的应用层接口，可以用于构建上位机。**
|
||||
* 对于固件中每一个使用 CRTP 协议的组件，在 cflib 中都有一个脚本与其对应。
|
||||
@@ -162,7 +164,7 @@ ESP-Drone 项目继承 Crazyflie 项目使用的 CRTP \(Crazy RealTime Protocol\
|
||||
|
||||
## 基于 CRTP 协议的应用开发
|
||||
|
||||
**各个平台工程模板**
|
||||
### 各个平台工程模板
|
||||
|
||||
1. [crazyflie2-ios-client](https://github.com/bitcraze/crazyflie2-ios-client)
|
||||
|
||||
@@ -174,10 +176,10 @@ ESP-Drone 项目继承 Crazyflie 项目使用的 CRTP \(Crazy RealTime Protocol\
|
||||
|
||||
[安卓版本开发指南](https://wiki.bitcraze.io/doc:crazyflie:dev:env:android)
|
||||
|
||||
**CFclient**
|
||||
### CFclient
|
||||
|
||||
CFclient 是 `crazeflie` 源工程的上位机，完全实现了 `CRTP` 协议中定义的功能，可以加快飞机的调试过程，ESP-Drone 项目对该上位机进行裁剪和调整，满足功能设计需求。
|
||||
|
||||
|
||||
|
||||
CFclient 具体使用说明可查阅：[CFclient](./cfclient.md)
|
||||
CFclient 具体使用说明可查阅：[CFclient](gettingstarted.html#pc-cfclient)
|
||||
@@ -1,150 +0,0 @@
|
||||
|
||||
## 已支持的控制器
|
||||
|
||||
ESP-Drone 控制系统代码来自 `crazyflie`，也继承了该工程的所有控制算法，需要注意的是，ESP-Drone 仅对 PID 控制器进行了参数整定和测试，换用其它控制器时，请在确保安全的情况下，自行进行参数整定。
|
||||
|
||||
|
||||
|
||||
详情请参考：[https://www.bitcraze.io/2020/02/out-of-control/](https://www.bitcraze.io/2020/02/out-of-control/)
|
||||
|
||||
可在代码中,通过修改 `controllerInit(ControllerType controller)` 的传入参数，对控制器进行切换。
|
||||
|
||||
也可通过实现以下控制器接口，添加自定义的控制器：
|
||||
|
||||
```
|
||||
static ControllerFcns controllerFunctions[] = {
|
||||
{.init = 0, .test = 0, .update = 0, .name = "None"}, // Any
|
||||
{.init = controllerPidInit, .test = controllerPidTest, .update = controllerPid, .name = "PID"},
|
||||
{.init = controllerMellingerInit, .test = controllerMellingerTest, .update = controllerMellinger, .name = "Mellinger"},
|
||||
{.init = controllerINDIInit, .test = controllerINDITest, .update = controllerINDI, .name = "INDI"},
|
||||
};
|
||||
```
|
||||
|
||||
## PID控制器
|
||||
|
||||
**控制原理**
|
||||
|
||||
PID 控制器（比例-积分-微分控制器），由比例单元（Proportional）、积分单元（Integral）和微分单元（Derivative）组成，分别对应当前误差、过去累计误差及未来误差，最终基于误差和误差的变化率对系统进行控制。PID 控制器由于具有负反馈修正作用，一般被认为是最适用的控制器。通过调整 PID 控制器的三类参数，可以调整系统对误差的反应快慢、控制器过冲的程度及系统震荡的程度，使系统达到最优状态。
|
||||
|
||||
在飞行器系统中，由于存在 `pitch` 、 `roll` 、 `yaw` 三个自由度，因此需要设计如下图所示的具有控制闭环的 PID 控制器。
|
||||
|
||||
|
||||
|
||||
> 图片来自 Sun Feb 08, 2015
|
||||
|
||||
* 其中每一个自由度都包括一个串级 PID 控制器，Rate 控制和 Attitude 控制，前者以角速度作为输入量，控制角度修正的速度，后者以拟合后的角度为输入量，控制飞机到达目标角度，两个控制器以不同的频率配合工作。当然，也可以选择只使用单级的 PID 控制，默认情况下 pitch 和 roll 自由度使用 Attitude 控制，yaw 使用 Rate 控制。
|
||||
|
||||
```
|
||||
可以在crtp_commander_rpyt.c中调整如下参数选择
|
||||
static RPYType stabilizationModeRoll = ANGLE; // Current stabilization type of roll (rate or angle)
|
||||
static RPYType stabilizationModePitch = ANGLE; // Current stabilization type of pitch (rate or angle)
|
||||
static RPYType stabilizationModeYaw = RATE; // Current stabilization type of yaw (rate or angle)
|
||||
```
|
||||
|
||||
**PID 参数整定**
|
||||
|
||||
1. 各个自由度修改为 `Rate `模式，进行 PID 参数的调整
|
||||
2. 各个自由度修改为 `Attitude `模式，进行 PID 参数的调整
|
||||
|
||||
详情请查阅：[PID tune](./pid_tune.md)
|
||||
|
||||
**实现代码**
|
||||
|
||||
```
|
||||
void controllerPid(control_t *control, setpoint_t *setpoint,
|
||||
const sensorData_t *sensors,
|
||||
const state_t *state,
|
||||
const uint32_t tick)
|
||||
{
|
||||
if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) { //该宏定义用于控制PID的计算频率，时间基准来自MPU6050触发的中断
|
||||
// Rate-controled YAW is moving YAW angle setpoint
|
||||
if (setpoint->mode.yaw == modeVelocity) { //rata模式,对yaw做修正
|
||||
attitudeDesired.yaw += setpoint->attitudeRate.yaw * ATTITUDE_UPDATE_DT;
|
||||
while (attitudeDesired.yaw > 180.0f)
|
||||
attitudeDesired.yaw -= 360.0f;
|
||||
while (attitudeDesired.yaw < -180.0f)
|
||||
attitudeDesired.yaw += 360.0f;
|
||||
} else { //attitude模式
|
||||
attitudeDesired.yaw = setpoint->attitude.yaw;
|
||||
}
|
||||
}
|
||||
|
||||
if (RATE_DO_EXECUTE(POSITION_RATE, tick)) { //位置控制
|
||||
positionController(&actuatorThrust, &attitudeDesired, setpoint, state);
|
||||
}
|
||||
|
||||
if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
|
||||
// Switch between manual and automatic position control
|
||||
if (setpoint->mode.z == modeDisable) {
|
||||
actuatorThrust = setpoint->thrust;
|
||||
}
|
||||
if (setpoint->mode.x == modeDisable || setpoint->mode.y == modeDisable) {
|
||||
attitudeDesired.roll = setpoint->attitude.roll;
|
||||
attitudeDesired.pitch = setpoint->attitude.pitch;
|
||||
}
|
||||
|
||||
attitudeControllerCorrectAttitudePID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw,
|
||||
attitudeDesired.roll, attitudeDesired.pitch, attitudeDesired.yaw,
|
||||
&rateDesired.roll, &rateDesired.pitch, &rateDesired.yaw);
|
||||
|
||||
// For roll and pitch, if velocity mode, overwrite rateDesired with the setpoint
|
||||
// value. Also reset the PID to avoid error buildup, which can lead to unstable
|
||||
// behavior if level mode is engaged later
|
||||
if (setpoint->mode.roll == modeVelocity) {
|
||||
rateDesired.roll = setpoint->attitudeRate.roll;
|
||||
attitudeControllerResetRollAttitudePID();
|
||||
}
|
||||
if (setpoint->mode.pitch == modeVelocity) {
|
||||
rateDesired.pitch = setpoint->attitudeRate.pitch;
|
||||
attitudeControllerResetPitchAttitudePID();
|
||||
}
|
||||
|
||||
// TODO: Investigate possibility to subtract gyro drift.
|
||||
attitudeControllerCorrectRatePID(sensors->gyro.x, -sensors->gyro.y, sensors->gyro.z,
|
||||
rateDesired.roll, rateDesired.pitch, rateDesired.yaw);
|
||||
|
||||
attitudeControllerGetActuatorOutput(&control->roll,
|
||||
&control->pitch,
|
||||
&control->yaw);
|
||||
|
||||
control->yaw = -control->yaw;
|
||||
}
|
||||
|
||||
if (tiltCompensationEnabled)
|
||||
{
|
||||
control->thrust = actuatorThrust / sensfusion6GetInvThrustCompensationForTilt();
|
||||
}
|
||||
else
|
||||
{
|
||||
control->thrust = actuatorThrust;
|
||||
}
|
||||
|
||||
if (control->thrust == 0)
|
||||
{
|
||||
control->thrust = 0;
|
||||
control->roll = 0;
|
||||
control->pitch = 0;
|
||||
control->yaw = 0;
|
||||
|
||||
attitudeControllerResetAllPID();
|
||||
positionControllerResetAllPID();
|
||||
|
||||
// Reset the calculated YAW angle for rate control
|
||||
attitudeDesired.yaw = state->attitude.yaw;
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
## Mellinger 控制器
|
||||
|
||||
Mellinger 控制器是一种“多合一” 控制器，它基于目标位置和目标位置速度矢量，直接计算出需要分配给所有电动机的所需推力。
|
||||
|
||||
详情可参考论文：[Minimum snap trajectory generation and control for quadrotors](https://ieeexplore.ieee.org/abstract/document/5980409)
|
||||
|
||||
## INDI 控制器
|
||||
|
||||
INDI 控制器是立即处理角速率以确定信任度的控制器，与传统的 PID 控制器相结合，对于角度处理相比串级 PID 控制器组合的速度要快。
|
||||
|
||||
详情可参考论文：[Adaptive Incremental Nonlinear Dynamic Inversion for Attitude Control of Micro Air Vehicles](https://arc.aiaa.org/doi/pdf/10.2514/1.G001490)
|
||||
|
||||
|
||||
@@ -0,0 +1,31 @@
|
||||
|
||||
# 开发指引
|
||||
|
||||
* [搭建开发环境](./getespidf.md)
|
||||
* [ESP-IDF 开发环境搭建](getespidf.html#esp-idf)
|
||||
* [ESP32(S2) 链接脚本修改](getespidf.html#esp32-s2)
|
||||
* [获取源代码](getespidf.html#id2)
|
||||
* [驱动程序](./drivers.md)
|
||||
* [通用设备](drivers.html#generall)
|
||||
* [I2C 总线设备](drivers.html#i2c-devices)
|
||||
* [SPI 总线设备](drivers.html#spi-devices)
|
||||
* [飞控系统](./system.md)
|
||||
* [系统启动流程](system.html#id2)
|
||||
* [系统任务管理](system.html#id3)
|
||||
* [关键任务介绍](system.html#id7)
|
||||
* [传感器硬件抽象](system.html#id9)
|
||||
* [传感器校准](system.html#id10)
|
||||
* [姿态计算](system.html#id15)
|
||||
* [控制算法](system.html#id19)
|
||||
* [通信协议](./communication.md)
|
||||
* [通信层级结构](communication.html#id2)
|
||||
* [Wi-Fi 通信](communication.html#wi-fi)
|
||||
* [UDP 通信](communication.html#udp)
|
||||
* [CRTP 协议](communication.html#crtp)
|
||||
* [基于 CRTP 协议的应用开发](communication.html#id10)
|
||||
|
||||
* [硬件参考](./hardware.md)
|
||||
* [已支持硬件列表](hardware.html#id3)
|
||||
* [ESP32_S2_Drone_V1_2](hardware.html#esp32-s2-drone-v1-2)
|
||||
* [ESPlane_V2_S2](hardware.html#esplane-v2-s2)
|
||||
* [ESPlane FC V1](hardware.html#esplane-fc-v1)
|
||||
+490
-12
@@ -1,30 +1,508 @@
|
||||
|
||||
# 驱动程序
|
||||
|
||||
## general
|
||||
|
||||
* adc
|
||||
* ADC
|
||||
|
||||
* buzzer
|
||||
* Buzzer
|
||||
|
||||
* led
|
||||
* Led
|
||||
|
||||
* motors
|
||||
* Motors
|
||||
|
||||
* [wifi](./communication.md#wi-fi-通信)
|
||||
* [Wi-Fi](./communication.md)
|
||||
|
||||
## i2c_devices
|
||||
|
||||
* [mpu6050](./mpu6050.md)
|
||||
### MPU6050
|
||||
|
||||
* [vl53l0](./vl53lxx.md)
|
||||
#### MPU6050 传感器特性
|
||||
|
||||
* [vl53l1](./vl53lxx.md)
|
||||
**工作原理**
|
||||
|
||||
* ms5611
|
||||
* 陀螺仪：当陀螺仪围绕任何感应轴旋转时，科里奥利效应就会产生电容式传感器检测到的振动。所得到的信号被放大，解调和滤波产生与角速度成比例的电压。
|
||||
* 电子加速度计：加速沿着一条特定轴在相应的检测质量上引起位移，引起电容式传感器检测电容的变化。
|
||||
|
||||
* hmc5883l
|
||||
**测量范围**
|
||||
|
||||
* eeprom
|
||||
* 可配置陀螺仪测量范围： ±250, ±500, ±1000, ±2000°/sec
|
||||
* 可配置加速度计测量范围： ±2g, ±4g, ±8g，±16g
|
||||
|
||||
**AUX I2C 接口**
|
||||
|
||||
* MPU-60X0 具有一个辅助 I2C 总线，用于与片外3轴数字磁力计或其他传感器进行通信。
|
||||
* 辅助 I2C 接口有两种工作模式：I2C Master Mode 或 Pass-Through Mode。
|
||||
|
||||
**MPU-60X0 FIFO**
|
||||
|
||||
* MPU-60X0 包含一个可通过串行接口访问的 1024 字节 FIFO 寄存器。 FIFO配置寄存器决定哪个数据写入FIFO。 可能的选择包括陀螺仪数据，加速计数据，温度读数，辅助传感器读数和 FSYNC 输入。
|
||||
|
||||
**数字低通滤波器（DLPF）**
|
||||
|
||||
* MPU6050自带低通滤波器，可以通过配置寄存器26控制低通滤波频段，减少高频干扰。但是会降低传感器输入速率（开启DLPF加速度计输出1kHZ，关闭DLPF可以输出8Khz）
|
||||
|
||||
**FSYNC帧同步采样引脚**
|
||||
|
||||
* 寄存器 26-EXT\_SYNC\_SET，用于配置外部帧同步引脚的采样
|
||||
|
||||
**数字运动处理器（DMP）**
|
||||
|
||||
* MPU6050 内部存在一个数字运动处理单元（Digital Motion Processor，DMP），可以计算四元数等，减轻主 CPU 压力。
|
||||
* DMP 可以通过引脚触发中断。
|
||||
|
||||
|
||||
|
||||
**MPU6050方向定义**
|
||||
|
||||
|
||||
|
||||
#### MPU6050 初始化步骤
|
||||
|
||||
1. 恢复寄存器默认值：设置 PWR\_MGMT\_1 bit7 为 1 ，恢复后 bit7 为 0 ，bit6 自动设置为 1，进入 sleep 模式
|
||||
2. 设置 PWR\_MGMT\_1 bit6 为0，唤醒传感器
|
||||
3. 设置时钟源
|
||||
4. 设置量程：分别设置陀螺仪和加速度计量程
|
||||
5. 设置采样率
|
||||
6. 设置数字低通滤波器（可选）
|
||||
17
|
||||
#### MPU6050 关键寄存器
|
||||
|
||||
**寄存器典型值**
|
||||
|
||||
| 寄存器 | 典型值 | 功能 |
|
||||
| :---: | :---: | :---: |
|
||||
| PWR\_MGMT\_1 | 0x00 | 正常启用 |
|
||||
| SMPLRT\_DIV | 0x07 | 陀螺仪采样率 125Hz |
|
||||
| CONFIG | 0x06 | 低通滤波器频率为 5Hz |
|
||||
| GYRO\_CONFIG | 0x18 | 陀螺仪不自检，输出满量程范围为 ± 2000 °/s |
|
||||
| ACCEL\_CONFIG | 0x01 | 加速度计不自检，输出的满量程范围为± 2g |
|
||||
|
||||
**寄存器117-设备地址-WHO\_AM\_I**
|
||||
|
||||
* \[6:1\] 保存设备地址，默认为 0x68，不反应 AD0 引脚值。
|
||||
|
||||
|
||||
|
||||
**寄存器107-电源管理1-PWR\_MGMT\_1**
|
||||
|
||||
|
||||
|
||||
* DEVICE\_RESET：设置为 1 时，寄存器值设置为默认。
|
||||
* SLEEP: 当该位置 1 时，该位将 MPU-60X0 置于睡眠模式。
|
||||
* CYCLE：当该位设置为 1 且 SLEEP 被禁止时，MPU-60X0 将循环在睡眠模式和唤醒之间以 LP\_WAKE\_CTRL（寄存器108）确定的速率从活动传感器获取单个样本数据。
|
||||
|
||||
**寄存器26-配置数字低通滤波器 -CONFIG**
|
||||
|
||||
|
||||
|
||||
* 数字低通滤波器（DLPF）取值与滤波频段关系:
|
||||
|
||||
|
||||
|
||||
**寄存器27 - 陀螺仪量程配置-GYRO\_CONFIG**
|
||||
|
||||
|
||||
|
||||
* XG\_ST: X轴陀螺仪自检
|
||||
* FS\_SEL：用于配置陀螺仪量程：
|
||||
|
||||
|
||||
|
||||
**寄存器28 - 加速度计量程配置-ACCEL\_CONFIG**
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
**寄存器25 - 采样速率分频器-SMPRT\_DIV**
|
||||
|
||||
该寄存器指定用于产生MPU-60X0采样率的陀螺仪输出速率的分频器。传感器寄存器输出，FIFO输出和DMP采样都基于采样率。采样率是通过将陀螺仪输出速率除以 SMPLRT\_DIV 产生的。
|
||||
|
||||
|
||||
|
||||
> Sample Rate = Gyroscope Output Rate / \(1 + SMPLRT\_DIV\) where Gyroscope Output Rate = 8kHz when the DLPF is disabled \(DLPF\_CFG = 0 or 7\), and 1kHz when the DLPF is enabled \(see Register 26\)
|
||||
|
||||
* 在不开启DLPF的情况下，设置 SMPLRT\_DIV 为 7 可以时芯片产生 1khz 的中断信号。
|
||||
|
||||
|
||||
|
||||
**寄存器59到64-加速度计测量值**
|
||||
|
||||
|
||||
|
||||
* 大数端存放：地址地位存放数据高位，地址高位存放数据地位。
|
||||
* 补码存放：因为测量值为有符号整数，因此采用补码方式存放
|
||||
|
||||
**寄存器65和66 - 温度测量**
|
||||
|
||||
|
||||
|
||||
**寄存器67至72 - 陀螺仪测量值**
|
||||
|
||||
|
||||
|
||||
### VL53LX
|
||||
|
||||
|
||||
#### VL53LXX 传感器特性
|
||||
|
||||
**工作原理**
|
||||
|
||||
VL53L0X / VL53L1X 芯片内部集成了激光发射器和 SPAD 红外接收器，通过探测光子发送和接收时间差，计算光子飞行距离，最远测量距离可达两米，适合中短距离测量的应用。
|
||||
|
||||
|
||||
|
||||
**测量区域 - ROI**
|
||||
|
||||
VL53L0X / VL53L1X 的测量值为测量区域中的最短距离，测量区域可以根据使用场景进行放大或收缩，较大的探测范围可能会引起测量值的波动。
|
||||
|
||||
> 测量区域的配置参见 2.4 ranging description 2.8 Sensing array optical center
|
||||
|
||||
|
||||
|
||||
**测量距离**
|
||||
|
||||
* VL53L0X 传感器存在 **3-4 cm 的盲区**，有效测量范围 3cm-200cm，精度 +-3%
|
||||
* VL53L1X 是 VL53L0X 的升级版本，探测距离可达 400 cm
|
||||
|
||||
|
||||
|
||||
* VL53LXX 测量距离与光线环境有关，黑暗环境下可获得更高的探测距离，在室外强光下，激光传感器可能会受到很大的干扰，导致测量精度降低，因此室外需要结合气压定高。
|
||||
|
||||
|
||||
|
||||
**测量频率**
|
||||
|
||||
* VL53L0X 响应频率最快可达 50 Hz，测量误差+-5%
|
||||
* VL53L1X I2C 最大时钟频率可达 400khz，上拉电阻需要根据电压和总线电容值选择，可以参见vl53l1x datasheet
|
||||
|
||||
|
||||
|
||||
* XSHUT 为输入引脚，用于模式选择（休眠），需要上拉电阻放置漏电流
|
||||
* GPIO1 为中断输出引脚，用于输出测量 dataready 中断
|
||||
|
||||
**工作模式**
|
||||
|
||||
通过设置 XSHUT 引脚的电平，可以切换传感器进入 HW Standby 模式或 SW Standby 模式，实现有条件的启动，降低待机功耗。如果主机放弃对传感器模式进行管理，可将 XSHUT 引脚默认为上拉。
|
||||
|
||||
* HW Standby ：XSHUT拉低时，传感器电源被关闭
|
||||
* SW Standby ：XSHUT拉高，进入boot和SW Standby 模式
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#### VL53LXX 初始化步骤
|
||||
|
||||
1. 等待硬件初始化完成
|
||||
2. 数据初始化
|
||||
3. 静态初始化，装载数据
|
||||
4. 设置测量距离模式
|
||||
5. 设置单次测量最长等待时间
|
||||
6. 设置测量频率（时间间隔）
|
||||
7. 设置测量区域 ROI（可选）
|
||||
8. 启动测量
|
||||
|
||||
```text
|
||||
/*init vl53l1 module*/
|
||||
void vl53l1_init()
|
||||
{
|
||||
|
||||
Roi0.TopLeftX = 0; //测量目标区 可选最小4*4，最大16*16
|
||||
Roi0.TopLeftY = 15;
|
||||
Roi0.BotRightX = 7;
|
||||
Roi0.BotRightY = 0;
|
||||
Roi1.TopLeftX = 8;
|
||||
Roi1.TopLeftY = 15;
|
||||
Roi1.BotRightX = 15;
|
||||
Roi1.BotRightY = 0;
|
||||
|
||||
int status = VL53L1_WaitDeviceBooted(Dev); //等待硬件初始化完成
|
||||
status = VL53L1_DataInit(Dev); //数据初始化，上电后立刻执行
|
||||
status = VL53L1_StaticInit(Dev); //静态初始化，装载参数
|
||||
status = VL53L1_SetDistanceMode(Dev, VL53L1_DISTANCEMODE_LONG);//设置测量模式
|
||||
status = VL53L1_SetMeasurementTimingBudgetMicroSeconds(Dev, 50000); //设置最长时间，根据测量模式确定
|
||||
status = VL53L1_SetInterMeasurementPeriodMilliSeconds(Dev, 100); //测量间隔
|
||||
|
||||
status = VL53L1_SetUserROI(Dev, &Roi0); //设置ROI
|
||||
status = VL53L1_StartMeasurement(Dev); //启动测量
|
||||
if(status) {
|
||||
printf("VL53L1_StartMeasurement failed \n");
|
||||
while(1);
|
||||
}
|
||||
|
||||
}
|
||||
```
|
||||
|
||||
* 以上初始化步骤除 VL53L1\_SetUserROI，其余不可少
|
||||
|
||||
#### VL53LXX 测距步骤
|
||||
|
||||
**轮寻测量模式**
|
||||
|
||||
轮训测量流程图：
|
||||
|
||||
|
||||
|
||||
* 注意在完成一次测量和读取后，需要使用`VL53L1_ClearInterruptAndStartMeasurement`清除中断标志并重新开始。
|
||||
* 轮训测量有两种方法，如上图所示，一种是阻塞方式（drivers polling mode），一种是非阻塞方式（Host polling mode），以下代码为阻塞测量方式。
|
||||
|
||||
```text
|
||||
/* Autonomous ranging loop*/
|
||||
static void
|
||||
AutonomousLowPowerRangingTest(void)
|
||||
{
|
||||
printf("Autonomous Ranging Test\n");
|
||||
|
||||
static VL53L1_RangingMeasurementData_t RangingData;
|
||||
VL53L1_UserRoi_t Roi1;
|
||||
int roi = 0;
|
||||
float left = 0, right = 0;
|
||||
if (0/*isInterrupt*/) {
|
||||
} else {
|
||||
do // polling mode
|
||||
{
|
||||
int status = VL53L1_WaitMeasurementDataReady(Dev); //等待测量结果
|
||||
if(!status) {
|
||||
status = VL53L1_GetRangingMeasurementData(Dev, &RangingData); //获取单次测量数据
|
||||
if(status==0) {
|
||||
if (roi & 1) {
|
||||
left = RangingData.RangeMilliMeter;
|
||||
printf("L %3.1f R %3.1f\n", right/10.0, left/10.0);
|
||||
} else
|
||||
right = RangingData.RangeMilliMeter;
|
||||
}
|
||||
if (++roi & 1) {
|
||||
status = VL53L1_SetUserROI(Dev, &Roi1);
|
||||
} else {
|
||||
status = VL53L1_SetUserROI(Dev, &Roi0);
|
||||
}
|
||||
status = VL53L1_ClearInterruptAndStartMeasurement(Dev); //释放中断
|
||||
}
|
||||
}
|
||||
while (1);
|
||||
}
|
||||
// return status;
|
||||
}
|
||||
```
|
||||
|
||||
**中断测量模式**
|
||||
|
||||
中断测量模式需要使用中断引脚 GPIO1，在数据 ready 时，GPIO1 引脚电平将被拉低，通知主机进行数据读取。
|
||||
|
||||
|
||||
|
||||
#### VL53LXX 传感器校准
|
||||
|
||||
如果在传感器接收器上方添加了光罩，或者传感器藏在透明的盖板背后，由于透光率的变化，需要对传感器进行校准，可以根据校准流程调用 API 编写校准程序，也可以使用官方提供的 GUI 上位机直接测量出校准值。
|
||||
|
||||
**使用官方 API 编写校准程序**
|
||||
|
||||
校准流程：调用顺序要完全一致。
|
||||
|
||||
|
||||
|
||||
```
|
||||
/*Calibration vl53l1 module*/
|
||||
static VL53L1_CalibrationData_t vl53l1_calibration(VL53L1_Dev_t *dev)
|
||||
{
|
||||
int status;
|
||||
int32_t targetDistanceMilliMeter = 703;
|
||||
VL53L1_CalibrationData_t calibrationData;
|
||||
status = VL53L1_WaitDeviceBooted(dev);
|
||||
status = VL53L1_DataInit(dev); //performs the device initialization
|
||||
status = VL53L1_StaticInit(dev); // load device settings specific for a given use case.
|
||||
status = VL53L1_SetPresetMode(dev,VL53L1_PRESETMODE_AUTONOMOUS);
|
||||
status = VL53L1_PerformRefSpadManagement(dev);
|
||||
status = VL53L1_PerformOffsetCalibration(dev,targetDistanceMilliMeter);
|
||||
status = VL53L1_PerformSingleTargetXTalkCalibration(dev,targetDistanceMilliMeter);
|
||||
status = VL53L1_GetCalibrationData(dev,&calibrationData);
|
||||
|
||||
if (status)
|
||||
{
|
||||
ESP_LOGE(TAG, "vl53l1_calibration failed \n");
|
||||
calibrationData.struct_version = 0;
|
||||
return calibrationData;
|
||||
|
||||
}else
|
||||
{
|
||||
ESP_LOGI(TAG, "vl53l1_calibration done ! version = %u \n",calibrationData.struct_version);
|
||||
return calibrationData;
|
||||
}
|
||||
|
||||
}
|
||||
```
|
||||
|
||||
**使用官方 GUI 上位机校准传感器**
|
||||
|
||||
官方提供了用于配置和校准传感器的 GUI 上位机，配合 ST 官方 `STM32F401RE nucleo` 开发板连接传感器，使用软件校准得到基准值后，初始化时填入的即可。
|
||||
|
||||
>
|
||||
|
||||
>[STSW-IMG008:Windows Graphical User Interface \(GUI\) for VL53L1X Nucleo packs. Works with P-NUCLEO-53L1A1 ](https://www.st.com/content/st_com/en/products/embedded-software/proximity-sensors-software/stsw-img008.html)
|
||||
|
||||
#### VL53L1x 例程
|
||||
|
||||
**例程说明**
|
||||
|
||||
1. 实现功能：通过VL53L1x 检测到高度变化（持续一秒），红灯亮起。高度恢复正常值（持续一秒），绿灯亮起。
|
||||
2. 可配置参数：通过make menuconfig 设置I2C 号码、端口号、LED端口号
|
||||
3. 例程解析见代码注释与用户手册
|
||||
|
||||
**注意事项**
|
||||
|
||||
4. 该例程只适用于VL53L1x，寄送的传感器为该型号。VL53L0x为老版本硬件，不适用本例程。
|
||||
5. 官方标称400cm测量距离，为黑暗环境下测得。室内正常灯光环境，可以保证10cm-260cm范围的有效测量
|
||||
6. 初始化函数vl53l1\_init（VL53L1\_Dev\_t \*） 中部分参数，需要根据实际使用环境确定，还有优化的空间。
|
||||
7. 传感器安装位置应确保在检测位置正上方
|
||||
8. 模块上电时自动矫正基准高度，如果基准高度有变化，需要重新上电重置参数
|
||||
|
||||
**例程仓库**
|
||||
|
||||
[esp32-vl53l1x-test](https://github.com/qljz1993/esp32-vl53l1x-test/tree/master) 或者：
|
||||
|
||||
```text
|
||||
git clone https://github.com/qljz1993/esp32-vl53l1x-test.git
|
||||
```
|
||||
|
||||
### MS5611
|
||||
|
||||
### HMC5883L
|
||||
|
||||
### EEPROM
|
||||
|
||||
## spi_devices
|
||||
|
||||
* [pmw3901](./pmw3901.md)
|
||||
### PMW3901
|
||||
|
||||
#### PMW3901传感器特性
|
||||
|
||||
PMW3901 是 PixArt 公司最新的高精度低功耗光学追踪模组，可直接获取 xy 方向运动速度信息，对地高度 8cm 以上实现有效测量 ，工作电流 < 9mA ， 工作电压 VDD(1.8~2.1VDC) VDDIO(1.8~3.6VDC)，使用 4 线 SPI 接口通信。
|
||||
|
||||
**主要参数**
|
||||
|
||||
| Parameter | Value |
|
||||
|--|--|
|
||||
| Supply Voltage (V) | VDD: 1.8 – 2.1 VDDIO: 1.8 – 3.6 |
|
||||
|Working Range (mm) | 80 to infinity |
|
||||
| Interface | 4-Wire SPI @ 2 Mhz|
|
||||
| Package Type | 28-pin COB Package with Lens Assembly:6 x 6 x 2.28 mm |
|
||||
|
||||
**封装和引脚图**
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
> 传感器工作电压较低，与3.3V的ＥＳＰ３２通信，需要ＶＤＤ和ＶＤＤＩＯ提供不同的电压
|
||||
|
||||
#### 上电启动流程
|
||||
|
||||
**Power-Up Sequence**
|
||||
Although PMW3901MB performs an internal power up self-reset, it is still recommended that the Power_Up_Reset register is written every time power is applied. The appropriate sequence is as follows:
|
||||
1. Apply power to VDDIO first and followed by VDD, with a delay of no more than 100ms in between each supply. Ensure all supplies are stable.
|
||||
2. Wait for at least 40 ms.
|
||||
3. Drive NCS high, and then low to reset the SPI port.
|
||||
4. Write 0x5A to Power_Up_Reset register (or alternatively, toggle the NRESET pin).
|
||||
5. Wait for at least 1 ms.
|
||||
6. Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion pin state.
|
||||
7. Refer Section 8.2 Performance Optimization Registers to configure the needed registers in order to achieve optimum performance of the chip.
|
||||
|
||||
|
||||
**Power-Down Sequence**
|
||||
PMW3901MB can be set to Shutdown mode by writing to Shutdown register. The SPI port should not be accessed when Shutdown mode is asserted, except the power-up command (writing 0x5A to register 0x3A). Other ICs on the same SPI bus can be accessed, as long as the chip’s NCS pin is not asserted.
|
||||
To de-assert Shutdown mode:
|
||||
8. Drive NCS high, and then low to reset the SPI port.
|
||||
9. Write 0x5A to Power_Up_Reset register (or alternatively, toggle the NRESET pin).
|
||||
10. Wait for at least 1 ms.
|
||||
11. Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion pin state.
|
||||
12. Refer Section 8.2 Performance Optimization Registers to configure the needed registers in order to achieve optimum performance of the chip.
|
||||
|
||||
|
||||
[pixart其他产品助力IOT](https://www.pixart.com/applications/11/Connected_Home_Appliances_%EF%BC%86_IoT)
|
||||
|
||||
#### 部分代码解读
|
||||
|
||||
**关键结构体**
|
||||
|
||||
```
|
||||
|
||||
typedef struct opFlow_s
|
||||
{
|
||||
float pixSum[2]; /*累积像素*/
|
||||
float pixComp[2]; /*像素补偿*/
|
||||
float pixValid[2]; /*有效像素*/
|
||||
float pixValidLast[2]; /*上一次有效像素*/
|
||||
float deltaPos[2]; /*2 帧之间的位移 单位 cm*/
|
||||
float deltaVel[2]; /*速度 单位 cm/s*/
|
||||
float posSum[2]; /*累积位移 单位 cm*/
|
||||
float velLpf[2]; /*速度低通 单位 cm/s*/
|
||||
bool isOpFlowOk; /*光流状态*/
|
||||
bool isDataValid; /*数据有效*/
|
||||
} opFlow_t;
|
||||
|
||||
```
|
||||
|
||||
* 累积像素，就是自四轴起飞后的累积像素；
|
||||
* 像素补偿，就是补偿由于飞机倾斜导致的像素误差；
|
||||
* 有效像素，指经过补偿的实际像素；
|
||||
* 2 帧之间的位移，这个就是由像素转换出来的实际位移，单位 cm；
|
||||
* 速度，这个速度是瞬时速度，由位移变化量微分得到，单位 cm/s；
|
||||
* 累积位移，实际位移，单位 cm速度低通，对速度进行低通，增加数据平滑性；
|
||||
* 光流状态，光流是否正常工作；
|
||||
* 数据有效，在一定高度范围内，数据有效；
|
||||
|
||||
```
|
||||
typedef struct motionBurst_s {
|
||||
union {
|
||||
uint8_t motion;
|
||||
struct {
|
||||
uint8_t frameFrom0 : 1;
|
||||
uint8_t runMode : 2;
|
||||
uint8_t reserved1 : 1;
|
||||
uint8_t rawFrom0 : 1;
|
||||
uint8_t reserved2 : 2;
|
||||
uint8_t motionOccured : 1;
|
||||
};
|
||||
};
|
||||
|
||||
uint8_t observation;
|
||||
int16_t deltaX;
|
||||
int16_t deltaY;
|
||||
|
||||
uint8_t squal;
|
||||
|
||||
uint8_t rawDataSum;
|
||||
uint8_t maxRawData;
|
||||
uint8_t minRawData;
|
||||
|
||||
uint16_t shutter;
|
||||
} __attribute__((packed)) motionBurst_t;
|
||||
```
|
||||
|
||||
* motion ：运动信息，可以根据不同的位去判断运动信息，包括帧判别，运行模式和运动信息检测等。
|
||||
* observation：这个是用于检测 IC 是否出现 EFT/B 或者 ESD 问题，传感器正常工作时，读取出来的值为0xBF。
|
||||
* deltaX, deltaY ：光流检测到图像的 X 和 Y 方向的运动信息。
|
||||
* squal ：指运动信息质量，简单说就是运动信息的可信度。
|
||||
* rawDataSum ：这个是原数据求和，可用作对一帧数据求平均值；maxRawData 和 minRawData ，是最大和最小原始数据；
|
||||
* shutter：是一个实时自动调整的值，目的是保证平均运动数据在正常可操作范围以内，这个值可以搭配 squal，用来判断运动信息是否可用。
|
||||
|
||||
#### 编程注意事项
|
||||
|
||||
1. 如果连续 1s 内光流数据都为 0，说明出现故障，需要做挂起光流任务等处理。
|
||||
2. 可以设置pitch为x，roll方向为y，需要注意因为传感器向下安装，且地面不动，**飞机向前走，图像是向后的。**
|
||||
3. 需要测量准确的高度，同于确定图像像素和实际距离的对应关系，这个高度会作为一个计算的参数（因此只有在定高模式稳定才能定点）光流手册 42 页计算关系。
|
||||
|
||||
```
|
||||
补充计算关系与代码实现
|
||||
```
|
||||
|
||||
4. 需要手动测试倾角补偿，效果是，能够通过补偿，使飞行器有一定的倾角时，传感器输出基本不变化
|
||||
|
||||
```
|
||||
补充测试过程
|
||||
```
|
||||
|
||||
5. 有了倾角补偿和运动累积像素，我们就可以得到实际累积像素，减去上次的实际像素，就可以得到 2 帧之间的变化像素，再乘以系数就可以得到 2 帧之间的位移变化，可以看到还有对系数的限制，当高度小于 5cm，光流就无法工作了，所以系数设置为 0。接着对这个位移积分得到四轴到起飞点的位移，对这个位移微分得到瞬时速度，对速度进行低通增加数据的平滑性，对速度进行限幅处理，增加数据安全性。
|
||||
6. 通过光流就得到了四轴的位置信息和速度信息，把这些位置信息和速度信息融合加速计（state_estimator.c），得到估测位置和速度，将估测位置和速度参与 PID 运算，即可用于水平方向位置控制，这部分内容请看 position_pid.c，源码里面可以直接看到位置环和速度环 PID 的处理过程，这样就可以实现水平定点控制了。
|
||||
|
||||
@@ -1,21 +0,0 @@
|
||||
|
||||
## 已经支持的姿态计算算法
|
||||
|
||||
* 互补滤波
|
||||
* 卡尔曼滤波
|
||||
|
||||
ESP-Drone 姿态计算代码来自 `crazyflie`，ESP-Drone 固件已经对互补滤波和卡尔曼滤波进行了实际测试，可以有效的计算飞行姿态，包括各个自由度的角度、角速度、和空间位置，为控制系统提供了可靠的状态输入。需要注意的是，在定点模式下，必须切换到卡尔曼滤波算法，才能保证工作正常。
|
||||
|
||||
crazyflie 状态估计：https://www.bitcraze.io/2020/01/state-estimation-to-be-or-not-to-be/
|
||||
|
||||
## 互补滤波
|
||||
|
||||
|
||||
|
||||
互补滤波中文说明可参考：https://zhuanlan.zhihu.com/p/34323865
|
||||
|
||||
## 卡尔曼滤波
|
||||
|
||||
|
||||
|
||||
卡尔曼滤波中文说明可参考：https://zhuanlan.zhihu.com/p/39912633
|
||||
@@ -1,26 +0,0 @@
|
||||
## ESP-IDF 开发环境搭建
|
||||
|
||||
请参照ESP-IDF 入门指南： [https://docs.espressif.com/projects/esp-idf/en/latest/esp32s2/get-started/index.html](https://docs.espressif.com/projects/esp-idf/en/latest/esp32s2/get-started/index.html)，按照步骤设置 ESP-IDF。
|
||||
|
||||
注意事项：
|
||||
* 请完成链接页面的所有步骤。
|
||||
* 请按照上面链接中的步骤构建一个或多个示例应用程序。
|
||||
|
||||
## ESP32(S2) 链接脚本修改
|
||||
|
||||
打开 ESP32(S2) 的链接脚本模板`${IDF_PATH}/components/esp32/ld/esp32.project.ld.in` 和 ` ${IDF_PATH}/components/esp32s2/ld/esp32s2.project.ld.in`, 将以下代码添加到 `.flash.rodata` 段的末尾.
|
||||
|
||||
```
|
||||
/* Parameters and log system datas */
|
||||
_param_start = .;
|
||||
KEEP(*(.param))
|
||||
KEEP(*(.param.*))
|
||||
_param_stop = .;
|
||||
. = ALIGN(4);
|
||||
_log_start = .;
|
||||
KEEP(*(.log))
|
||||
KEEP(*(.log.*))
|
||||
_log_stop = .;
|
||||
. = ALIGN(4);
|
||||
```
|
||||
以上代码可以实现，将具有 `.param.*` 或 `.log.*` 段属性的变量，放置在连续的存储区域，从而加快变量遍历速度。
|
||||
@@ -1,3 +1,33 @@
|
||||
|
||||
# 搭建开发环境
|
||||
|
||||
## ESP-IDF 环境搭建
|
||||
|
||||
请参照ESP-IDF 入门指南： [https://docs.espressif.com/projects/esp-idf/en/latest/esp32s2/get-started/index.html](https://docs.espressif.com/projects/esp-idf/en/latest/esp32s2/get-started/index.html)，按照步骤设置 ESP-IDF。
|
||||
|
||||
注意事项：
|
||||
* 请完成链接页面的所有步骤。
|
||||
* 请按照上面链接中的步骤构建一个或多个示例应用程序。
|
||||
|
||||
## ESP32(S2) 链接脚本修改
|
||||
|
||||
打开 ESP32(S2) 的链接脚本模板`${IDF_PATH}/components/esp32/ld/esp32.project.ld.in` 和 ` ${IDF_PATH}/components/esp32s2/ld/esp32s2.project.ld.in`, 将以下代码添加到 `.flash.rodata` 段的末尾.
|
||||
|
||||
```
|
||||
/* Parameters and log system datas */
|
||||
_param_start = .;
|
||||
KEEP(*(.param))
|
||||
KEEP(*(.param.*))
|
||||
_param_stop = .;
|
||||
. = ALIGN(4);
|
||||
_log_start = .;
|
||||
KEEP(*(.log))
|
||||
KEEP(*(.log.*))
|
||||
_log_stop = .;
|
||||
. = ALIGN(4);
|
||||
```
|
||||
以上代码可以实现，将具有 `.param.*` 或 `.log.*` 段属性的变量，放置在连续的存储区域，从而加快变量遍历速度。
|
||||
|
||||
## 获取项目源代码
|
||||
|
||||
**测试版本代码，目前放在 gitlab 仓库，使用 git 工具获取**
|
||||
@@ -54,7 +84,7 @@ git clone //TODO:补充github仓库
|
||||
└── sdkconfig.defaults | 默认参数
|
||||
```
|
||||
|
||||
**详情可查阅**：[espdrone_file_structure](../../_static/espdrone_file_structure.pdf)
|
||||
**详情可查阅**：[espdrone_file_structure](./_static/espdrone_file_structure.pdf)
|
||||
|
||||
## 源代码风格
|
||||
|
||||
@@ -0,0 +1,268 @@
|
||||
|
||||
# 快速入门
|
||||
|
||||
## 项目简介
|
||||
|
||||
ESP-Drone 是基于乐鑫 ESP32/ESP32-S2 开发的小型无人机解决方案，可使用手机 APP 或游戏手柄通过 Wi-Fi 网络进行连接和控制，目前已支持自稳定飞行、定高飞行、定点飞行等多种模式。该方案硬件结构简单，代码架构清晰，支持功能扩展，可用于 STEAM 教育等领域。项目部分代码来自 Crazyflie 开源工程，继承 GPL3.0 开源协议。
|
||||
|
||||
|
||||
|
||||
* 支持自稳定模式-stabilize：自动控制机身水平，保持平稳飞行
|
||||
* 支持定高模式-heighthold：自动控制油门输出，保持固定高度
|
||||
* 支持定点模式-positionhold：自动控制机身角度，保持固定空间位置
|
||||
* 支持 PC 上位机调试：使用 cfclient 上位机进行静态/动态调试
|
||||
* 支持 APP 控制：使用手机 APP 通过 Wi-Fi 轻松控制
|
||||
* 支持游戏手柄控制-gamepad：通过 cfclient 使用游戏手柄轻松控制
|
||||
|
||||
**ESP-Drone 2.0 使用模块化的设计思路，由主控板和扩展板组成：**
|
||||
|
||||
* **主控制板**：搭载支持基础飞行的必要元器件，提供硬件扩展接口
|
||||
* **扩展板**：基于硬件扩展接口，用于支持高级飞行功能或其他 DIY 功能
|
||||
|
||||
|序号| 模块名 | 主要元器件 | 功能 | 接口 |安装位置 |
|
||||
|--|--|--|--|--|--|
|
||||
|1| **主控制板-ESP32-S2** | ESP32-S2-WROVER + MPU6050| 基础飞行 |提供 I2C SPI GPIO 扩展接口 ||
|
||||
|2|扩展板- **定点模块** | PMW3901 + VL53L1X | 室内定点飞行 | SPI + I2C | 底部，面向地面 |
|
||||
|3| 扩展板-**气压定高模块** | MS5611 气压 | 气压定高 | I2C 或 MPU6050从机|顶部或底部 |
|
||||
|4| 扩展板-指南针模块 | HMC5883 罗盘 | 无头模式等高级模式 | I2C 或 MPU6050从机|顶部或底部 |
|
||||
|
||||
详情可查阅：[hardware](./hardware.md)
|
||||
|
||||
## ESP-IDF 简介
|
||||
|
||||
ESP-IDF 是乐鑫为 ESP32/ESP32-S2 提供的物联网开发框架。
|
||||
|
||||
* ESP-IDF 包含一系列库及头文件，提供了基于 ESP32/ESP32-S2 构建软件项目所需的核心组件。
|
||||
* ESP-IDF 还提供了开发和量产过程中最常用的工具及功能，例如：构建、烧录、调试和测量等。
|
||||
|
||||
详情可查阅：[ESP-IDF Get Started](https://docs.espressif.com/projects/esp-idf/en/latest/esp32s2/get-started/index.html)
|
||||
|
||||
## Crazyflie 简介
|
||||
|
||||
Crazyflie 是来自 bitcraze 开源工程的四旋翼飞行器。
|
||||
|
||||
* Crazyflie 支持多种传感器组合，可以轻松实现定高模式、定点模式等高级飞行模式。
|
||||
* Crazyflie 基于 FreeRTOS 编写，将复杂的无人机系统，分解成多个具有不同优先级的软件任务。
|
||||
* Crazyflie 设计了功能完备的 cfclient 上位机和 CRTP 通信协议，便于实现调试、测量和控制。
|
||||
|
||||
|
||||
|
||||
>图片来源：A swarm of drones exploring the environment, avoiding obstacles and each other. \(Guus Schoonewille, TU Delft\)
|
||||
|
||||
详情可查阅 crazyflie 官网：https://www.bitcraze.io/
|
||||
|
||||
## 准备工作
|
||||
|
||||
### 硬件组装
|
||||
|
||||
ESP-Drone_S2_V1.2：
|
||||
|
||||
|
||||
|
||||
详细的硬件介绍和引脚资源分配可查阅：[hardware](./hardware.md)
|
||||
|
||||
### 安装 ESPDroe APP
|
||||
|
||||
**Android APP 下载：**
|
||||
|
||||
[https://github.com/qljz1993/DailyMD_public/raw/master/esplanes2_test/ESP-Drone-android-debug.apk](https://github.com/qljz1993/DailyMD_public/raw/master/esplanes2_test/ESP-Drone-android-debug.apk)
|
||||
|
||||
**IOS APP 扫码下载：**
|
||||
|
||||
|
||||
|
||||
### 安装 CFClient
|
||||
|
||||
> 安装 CFClient 为可选步骤，用于实现高级调试，非必须使用
|
||||
|
||||
|
||||
|
||||
**1.安装crtp协议支持包**
|
||||
|
||||
1.1 下载源代码
|
||||
|
||||
```text
|
||||
git clone -b dev_esplane https://github.com/qljz1993/crazyflie-lib-python.git
|
||||
```
|
||||
|
||||
1.2 进入源码目录，安装依赖
|
||||
|
||||
```text
|
||||
pip3 install -r requirements.txt
|
||||
```
|
||||
|
||||
1.3 安装crtp包
|
||||
|
||||
```text
|
||||
pip3 install -e .
|
||||
```
|
||||
|
||||
**2.安装cfclient**
|
||||
|
||||
2.1 下载源代码
|
||||
|
||||
```text
|
||||
git clone -b dev_esplane https://github.com/qljz1993/crazyflie-clients-python.git
|
||||
```
|
||||
|
||||
2.2 进入源码目录，安装依赖
|
||||
|
||||
```text
|
||||
sudo apt-get install python3 python3-pip python3-pyqt5 python3-pyqt5.qtsvg
|
||||
```
|
||||
|
||||
2.3 安装cfclient客户端
|
||||
|
||||
```text
|
||||
pip3 install -e .
|
||||
```
|
||||
|
||||
2.4 启动客户端
|
||||
|
||||
|
||||
```text
|
||||
python3 ./bin/cfclient
|
||||
```
|
||||
|
||||
**3.配置遥控器**
|
||||
|
||||
|
||||
|
||||
3.1 配置4个控制轴 `Roll 、Pitch、Yaw、Thrust`
|
||||
|
||||
3.2 配置一个按键为`Assisted control` ，用于飞行模式切换
|
||||
|
||||
## 手机 APP 使用指引
|
||||
|
||||
１. WiFi 连接
|
||||
|
||||
SSID：ESPDRONE_XXXX (XXXX根据 MAC 设置） PASSWORD：12345678
|
||||
|
||||
２. 个性化设置
|
||||
|
||||
```
|
||||
默认配置：
|
||||
|
||||
Flight control settings
|
||||
1. Mode: Mode2
|
||||
2. Deadzone: 0.2
|
||||
3. Roll trim: 0.0
|
||||
4. Pitch trim: 0.0
|
||||
5. Advanced flight control : true
|
||||
6. Advanced flight control preferences
|
||||
1. max roll/pitch angle: 15
|
||||
2. max yaw angle: 90
|
||||
3. max thrust: 90
|
||||
4. min thrust: 25
|
||||
5. X-Mode: true
|
||||
Controller settings
|
||||
1. use full travel for thrust: false
|
||||
2. virtual joystick size: 100
|
||||
App settings
|
||||
1. Screen rotation lock: true
|
||||
2. full screen mode:true
|
||||
3. show console: true
|
||||
```
|
||||
|
||||
３. 控制飞行
|
||||
1. 打开通信连接，小飞机绿灯将闪烁
|
||||
2. 轻推油门，起飞
|
||||
|
||||
|
||||
|
||||
|
||||
## PC CFClient 使用指引
|
||||
|
||||
CFclient 是 `crazeflie` 源工程的上位机，完全实现了 `CRTP` 协议中定义的功能，可以加快飞机的调试过程，ESP-Drone 项目对该上位机进行裁剪和调整，满足功能设计需求。
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
项目中有很多相关的文件，例如配置文件和缓存文件，其中JSON文件用来存储配置信息。关于配置信息中内容的解读：[User configuration file](https://www.bitcraze.io/docs/crazyflie-clients-python/master/dev_info_client/)
|
||||
|
||||
### 飞行设置
|
||||
|
||||
**基本飞行设置 Basic Flight Control**
|
||||
|
||||
1. 飞行模式 Flight mode selector \(Normal and Advanced\)
|
||||
* 基本模式 _Normal: 初学者使用
|
||||
* 高级模式 _Advanced: 设置解锁最大角度，设置最大油门。
|
||||
2. 自动模式 Assisted mode selection.
|
||||
* 定海拔模式 _Altitude hold_: 保持飞行海拔，需要气压计支持。
|
||||
* 定点模式 _Position hold_: 保持当前位置，需要光流和TOF支持。
|
||||
* 定高模式 _Height hold_: 保持相对高度, 触发时保持高于地面 40cm ，需要TOF支持。
|
||||
* 悬停模式 _Hover : 触发时保持高于地面 40cm，并悬停在起飞点，需要光流和TOF支持。
|
||||
3. 角度修正 trim
|
||||
* Roll Trim : 翻滚角修正，用于弥补传感器水平安装误差
|
||||
* Pitch Trim : 俯仰角修正，用于弥补传感器水平安装误差
|
||||
|
||||
> 在自动模式下，油门摇杆变为高度控制摇杆
|
||||
|
||||
**高级飞行设置 Advanced Flight Control**
|
||||
|
||||
1. 最大倾角 _Max angle: 设置最大允许的俯仰和翻滚角度 roll/pitch
|
||||
2. 最大自选速度 _Max yaw rate: 设置允许的偏航速度 yaw
|
||||
3. 最大油门 _Max thrust: 设置最大油门
|
||||
4. 最小油门 _Min thrust: 设置最小油门
|
||||
5. 回转极限 _Slew limit: 回转极限。防止油门骤降，油门低于该值时，将被平滑的接管。 Set the percentage where the thrust is slew controlled \(the thrust value lowering will be limited\). This makes the Crazyflie a bit easier to fly for beginners
|
||||
6. 回转率 _Slew rate: 油门到回转极限时，最大的油门值。When the thrust is below the slew limit, this is the maximum rate of lowering the thrust
|
||||
|
||||
**遥控器设置 Configure input device**
|
||||
|
||||
按照提示绑定遥控器摇杆与各个控制通道：
|
||||
|
||||
|
||||
|
||||
**飞行数据 Fligt Data**
|
||||
|
||||
驾驶仪可以看到当前飞机姿态，右下方显示对应的详细数据。
|
||||
1. 目标角度 Target
|
||||
2. 测量角度 Actual
|
||||
3. 当前油门值 Thrust
|
||||
4. 电机实际输出 M1\M2\M3\M4
|
||||
|
||||
### 在线参数修改
|
||||
|
||||
**注意事项**
|
||||
|
||||
1. 修改的参数实时生效，避免了频繁烧录固件。
|
||||
2. 可在代码中通过宏定义配置那些参数可被上位机实时修改。
|
||||
3. 注意参数在线修改仅用于调试，掉电不保存。
|
||||
|
||||
**在线调整 PID 参数**
|
||||
|
||||
|
||||
|
||||
### 飞行数据监控
|
||||
|
||||
**配置要监控的参数**
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
**实时波形绘制**
|
||||
|
||||
陀螺仪加速度计实时数据监测：
|
||||
|
||||
|
||||
|
||||
|
||||
## 螺旋桨方向
|
||||
|
||||
* 注意按照下图所示位置，安装 A B 螺旋桨
|
||||
* 注意检查飞机上电自检时，螺旋桨转向是否正确？
|
||||
|
||||
|
||||
|
||||
## 起飞前检查
|
||||
|
||||
* 将飞机头部朝前放置，尾部天线朝向自己
|
||||
* 注意将飞机置于水平面上，机身稳定时上电
|
||||
* 注意观察上位机水平面是否置平？
|
||||
* 注意观察通信建立以后，小飞机尾部绿灯是否快速闪速
|
||||
* 注意观察小飞机头部红灯是否熄灭，亮起代表电量不足
|
||||
* 轻推左手小油门，检查飞机是否能快速响应
|
||||
* 轻推右手方向，检查方向控制是否正确
|
||||
* 起飞吧！
|
||||
+24
-19
@@ -1,7 +1,9 @@
|
||||
|
||||
# 硬件介绍
|
||||
|
||||
## 已支持硬件
|
||||
|
||||
**已支持硬件清单**
|
||||
### 已支持硬件清单
|
||||
|
||||
| 开发板名 | 主要配置 | 备注 |
|
||||
|:--:|:--:|:--:|
|
||||
@@ -9,7 +11,7 @@
|
||||
|ESPlane_V2_S2|ESP32-S2-WROVER + MPU6050|需安装脚架|
|
||||
|ESPlane_FC_V1|ESP32-S2-WROOM-32D + MPU6050|需安装机架|
|
||||
|
||||
**硬件切换方法**
|
||||
### 硬件切换方法
|
||||
|
||||
* `esp_drone` 仓库代码已支持多种硬件，可通过 `menuconfig` 进行切换。
|
||||
|
||||
@@ -33,12 +35,13 @@
|
||||
|
||||
|
||||
|
||||
主板原理图 ： [SCH_Mainboard_ESP32_S2_Drone_V1_2](../../../hardware/ESP32_S2_Drone_V1_2/SCH_Mainboard_ESP32_S2_Drone_V1_2.pdf)
|
||||
主板原理图 ： [SCH_Mainboard_ESP32_S2_Drone_V1_2](./_static/ESP32_S2_Drone_V1_2/SCH_Mainboard_ESP32_S2_Drone_V1_2.pdf)
|
||||
|
||||
主板 PCB ： [PCB_Mainboard_ESP32_S2_Drone_V1_2](../../../hardware/ESP32_S2_Drone_V1_2/PCB_Mainboard_ESP32_S2_Drone_V1_2.pdf)
|
||||
主板 PCB ： [PCB_Mainboard_ESP32_S2_Drone_V1_2](./_static/ESP32_S2_Drone_V1_2/PCB_Mainboard_ESP32_S2_Drone_V1_2.pdf)
|
||||
|
||||
### 基础配置
|
||||
|
||||
#### 基础配置清单
|
||||
|
||||
|
||||
| 基础配置清单 | 数量 | 备注 |
|
||||
@@ -54,19 +57,19 @@
|
||||
|
||||
>注意：更换 720 电机之后，需要在 `menuconfig->ESPDrone Config->motors config` 将 `motor type` 修改为 `brushed 720 motor`
|
||||
|
||||
**主控制器**
|
||||
#### 主控制器
|
||||
|
||||
| 芯片型号 | 模组型号 | 备注 |
|
||||
|--|--|--|
|
||||
| ESP32-S2 | ESP32-S2-WROVER | 模组内置 4MB flash，2MB PSRAM |
|
||||
|
||||
**传感器**
|
||||
#### 传感器
|
||||
|
||||
| 传感器 | 接口 | 备注 |
|
||||
|--|--|--|
|
||||
| MPU6050 | I2C0 | 主板传感器 |
|
||||
|
||||
**指示灯**
|
||||
#### 指示灯
|
||||
|
||||
| State | LED | Action |
|
||||
|--|--|--|
|
||||
@@ -76,7 +79,7 @@
|
||||
|UDP_RX|GREEN|BLINK|
|
||||
|LOW_POWER|RED|SOLID|
|
||||
|
||||
**按键**
|
||||
#### 按键
|
||||
|
||||
| Button | IO | Function |
|
||||
|--|--|--|
|
||||
@@ -84,7 +87,7 @@
|
||||
|SW2|EN|Reset|
|
||||
|
||||
|
||||
**主板 IO 定义**
|
||||
#### 主板 IO 定义
|
||||
|
||||
| 引脚 | 功能 | 备注 |
|
||||
| :---: | :---: | :---: |
|
||||
@@ -109,7 +112,7 @@
|
||||
| GPIO2 | ADC\_7\_BAT | VBAT/2 |
|
||||
| GPIO1 |EXT_IO1 | |
|
||||
|
||||
**摄像头接口**
|
||||
#### 摄像头接口
|
||||
|
||||
| 引脚 | 功能 | 备注 |
|
||||
| :---: | :---: | :---: |
|
||||
@@ -137,7 +140,7 @@
|
||||
扩展板原理图 ：待发布
|
||||
扩展板 PCB ：待发布
|
||||
|
||||
**扩展板 IO 定义**
|
||||
#### 扩展板 IO 定义
|
||||
|
||||
| 左引脚 | IO |功能 | 右引脚 | IO |功能|
|
||||
| :---: | :---: | :---: | :---: | :---: | :---:|
|
||||
@@ -154,22 +157,24 @@
|
||||
|
||||
|
||||
|
||||
主板原理图 ： [SCH_ESPlane_V2_S2](../../../hardware/ESPlane_V2_S2/SCH_ESPlane_V2_S2.pdf)
|
||||
主板原理图 ： [SCH_ESPlane_V2_S2](./_static/ESPlane_V2_S2/SCH_ESPlane_V2_S2.pdf)
|
||||
|
||||
主板 PCB ： [PCB_ESPlane_V2_S2](../../../hardware/ESPlane_V2_S2/PCB_ESPlane_V2_S2.pdf)
|
||||
主板 PCB ： [PCB_ESPlane_V2_S2](./_static/ESPlane_V2_S2/PCB_ESPlane_V2_S2.pdf)
|
||||
|
||||
|
||||
## ESPlane FC V1
|
||||
|
||||
|
||||
|
||||
主板原理图 ： [Schematic_ESPlane_FC_V1](../../../hardware/ESPlane_FC_V1/Schematic_ESPlane_FC_V1.pdf)
|
||||
主板原理图 ： [Schematic_ESPlane_FC_V1](./_static/ESPlane_FC_V1/Schematic_ESPlane_FC_V1.pdf)
|
||||
|
||||
主板 PCB ： [PCB_ESPlane_FC_V1](../../../hardware/ESPlane_FC_V1/PCB_ESPlane_FC_V1.pdf)
|
||||
主板 PCB ： [PCB_ESPlane_FC_V1](./_static/ESPlane_FC_V1/PCB_ESPlane_FC_V1.pdf)
|
||||
|
||||
|
||||
### 基础配置
|
||||
|
||||
#### 基础配置清单
|
||||
|
||||
| 配置清单 | 数量 | 备注 |
|
||||
|:--:|:--:|:--:|
|
||||
|主板|1|ESP32-WROOM-32D + MPU6050|
|
||||
@@ -179,13 +184,13 @@
|
||||
|300 mAh 1s 锂电池|1||
|
||||
|1s 锂电池充电板|1|||
|
||||
|
||||
**传感器**
|
||||
#### 传感器
|
||||
|
||||
| Sensor | Interface | Comment |
|
||||
|--|--|--|
|
||||
| MPU6050 | I2C0 | must |
|
||||
|
||||
**指示灯**
|
||||
#### 指示灯
|
||||
|
||||
```
|
||||
#define LINK_LED LED_BLUE
|
||||
@@ -203,7 +208,7 @@
|
||||
|SYSTEM READY|BLUE|SOLID|
|
||||
|UDP_RX|GREEN|BLINK|
|
||||
|
||||
**主板 IO 定义**
|
||||
#### 主板 IO 定义
|
||||
|
||||
| 引脚 | 功能 | 备注 |
|
||||
| :---: | :---: | :---: |
|
||||
@@ -227,7 +232,7 @@
|
||||
|
||||
### 扩展配置
|
||||
|
||||
**ESPlane + pmw3901 引脚配置**
|
||||
#### ESPlane + pmw3901 引脚配置
|
||||
|
||||
| 引脚 | 功能 | 备注 |
|
||||
| :---: | :---: | :---: |
|
||||
|
||||
+27
-11
@@ -2,17 +2,33 @@
|
||||
ESP-Drone
|
||||
===========
|
||||
|
||||
ESP-Drone 是基于乐鑫 `ESP32-S2 / ESP32` 开发的小型无人机解决方案，可使用手机 APP 或游戏手柄通过 Wi-Fi 网络进行连接和控制，目前已支持自稳定飞行、定高飞行、定点飞行等多种模式。该方案硬件结构简单，代码架构清晰完善，方便功能扩展，可用于STEAM教育等领域。控制系统代码来自 Crazyflie 开源工程，使用GPL3.0开源协议。
|
||||
|
||||
本文档包含 ESP-Drone 基本的开发和使用说明
|
||||
|
||||
================== ================== ==================
|
||||
|快速入门|_ |开发指引|_ |硬件参考|_
|
||||
------------------ ------------------ ------------------
|
||||
`快速入门`_ `开发指引`_ `硬件参考`_
|
||||
================== ================== ==================
|
||||
|
||||
.. |快速入门| image:: ../../_static/get-started.png
|
||||
.. _快速入门: gettingstarted.html
|
||||
|
||||
.. |开发指引| image:: ../../_static/api-reference.png
|
||||
.. _开发指引: developerguide.html
|
||||
|
||||
.. |硬件参考| image:: ../../_static/hw-reference.png
|
||||
.. _硬件参考: hardware.html
|
||||
|
||||
.. toctree::
|
||||
:hidden:
|
||||
|
||||
快速入门 <0_gettingstarted>
|
||||
选型参考 <1_user_guide>
|
||||
|
||||
|
||||
THANKS
|
||||
``````````
|
||||
|
||||
1. Thanks to the Bitcraze for the great [Crazyflie project](https://www.bitcraze.io/%20)
|
||||
2. Thanks to Espressif for the powerful [ESP-IDF environment](https://docs.espressif.com/projects/esp-idf/en/latest/esp32s2/get-started/index.html)
|
||||
3. Thanks to WhyEngineer for the useful [ESP-DSP lib](https://github.com/whyengineer/esp32-lin/tree/master/components/dsp_lib)
|
||||
|
||||
快速入门 <gettingstarted>
|
||||
环境搭建 <getespidf>
|
||||
开发指引 <developerguide>
|
||||
硬件参考 <hardware.md>
|
||||
驱动程序 <drivers.md>
|
||||
飞控系统 <system.md>
|
||||
通信协议 <communication.md>
|
||||
声明 <notice.md>
|
||||
|
||||
@@ -1,131 +0,0 @@
|
||||
|
||||
## MPU6050 传感器特性
|
||||
|
||||
**工作原理**
|
||||
|
||||
* 陀螺仪：当陀螺仪围绕任何感应轴旋转时，科里奥利效应就会产生电容式传感器检测到的振动。所得到的信号被放大，解调和滤波产生与角速度成比例的电压。
|
||||
* 电子加速度计：加速沿着一条特定轴在相应的检测质量上引起位移，引起电容式传感器检测电容的变化。
|
||||
|
||||
**测量范围**
|
||||
|
||||
* 可配置陀螺仪测量范围： ±250, ±500, ±1000, ±2000°/sec
|
||||
* 可配置加速度计测量范围： ±2g, ±4g, ±8g，±16g
|
||||
|
||||
**AUX I2C 接口**
|
||||
|
||||
* MPU-60X0 具有一个辅助 I2C 总线，用于与片外3轴数字磁力计或其他传感器进行通信。
|
||||
* 辅助 I2C 接口有两种工作模式：I2C Master Mode 或 Pass-Through Mode。
|
||||
|
||||
**MPU-60X0 FIFO**
|
||||
|
||||
* MPU-60X0 包含一个可通过串行接口访问的 1024 字节 FIFO 寄存器。 FIFO配置寄存器决定哪个数据写入FIFO。 可能的选择包括陀螺仪数据，加速计数据，温度读数，辅助传感器读数和 FSYNC 输入。
|
||||
|
||||
**数字低通滤波器（DLPF）**
|
||||
|
||||
* MPU6050自带低通滤波器，可以通过配置寄存器26控制低通滤波频段，减少高频干扰。但是会降低传感器输入速率（开启DLPF加速度计输出1kHZ，关闭DLPF可以输出8Khz）
|
||||
|
||||
**FSYNC帧同步采样引脚**
|
||||
|
||||
* 寄存器 26-EXT\_SYNC\_SET，用于配置外部帧同步引脚的采样
|
||||
|
||||
**数字运动处理器（DMP）**
|
||||
|
||||
* MPU6050 内部存在一个数字运动处理单元（Digital Motion Processor，DMP），可以计算四元数等，减轻主 CPU 压力。
|
||||
* DMP 可以通过引脚触发中断。
|
||||
|
||||
|
||||
|
||||
**MPU6050方向定义**
|
||||
|
||||
|
||||
|
||||
## MPU6050 初始化步骤
|
||||
|
||||
1. 恢复寄存器默认值：设置 PWR\_MGMT\_1 bit7 为 1 ，恢复后 bit7 为 0 ，bit6 自动设置为 1，进入 sleep 模式
|
||||
2. 设置 PWR\_MGMT\_1 bit6 为0，唤醒传感器
|
||||
3. 设置时钟源
|
||||
4. 设置量程：分别设置陀螺仪和加速度计量程
|
||||
5. 设置采样率
|
||||
6. 设置数字低通滤波器（可选）
|
||||
17
|
||||
## MPU6050 关键寄存器
|
||||
|
||||
### 寄存器典型值
|
||||
|
||||
| 寄存器 | 典型值 | 功能 |
|
||||
| :---: | :---: | :---: |
|
||||
| PWR\_MGMT\_1 | 0x00 | 正常启用 |
|
||||
| SMPLRT\_DIV | 0x07 | 陀螺仪采样率 125Hz |
|
||||
| CONFIG | 0x06 | 低通滤波器频率为 5Hz |
|
||||
| GYRO\_CONFIG | 0x18 | 陀螺仪不自检，输出满量程范围为 ± 2000 °/s |
|
||||
| ACCEL\_CONFIG | 0x01 | 加速度计不自检，输出的满量程范围为± 2g |
|
||||
|
||||
### 寄存器117-设备地址-WHO\_AM\_I
|
||||
|
||||
* \[6:1\] 保存设备地址，默认为 0x68，不反应 AD0 引脚值。
|
||||
|
||||
|
||||
|
||||
### 寄存器107-电源管理1-PWR\_MGMT\_1
|
||||
|
||||
|
||||
|
||||
* DEVICE\_RESET：设置为 1 时，寄存器值设置为默认。
|
||||
* SLEEP: 当该位置 1 时，该位将 MPU-60X0 置于睡眠模式。
|
||||
* CYCLE：当该位设置为 1 且 SLEEP 被禁止时，MPU-60X0 将循环在睡眠模式和唤醒之间以 LP\_WAKE\_CTRL（寄存器108）确定的速率从活动传感器获取单个样本数据。
|
||||
|
||||
### 寄存器26-配置数字低通滤波器 -CONFIG
|
||||
|
||||
|
||||
|
||||
* 数字低通滤波器（DLPF）取值与滤波频段关系:
|
||||
|
||||
|
||||
|
||||
### 寄存器27 - 陀螺仪量程配置-GYRO\_CONFIG
|
||||
|
||||
|
||||
|
||||
* XG\_ST: X轴陀螺仪自检
|
||||
* FS\_SEL：用于配置陀螺仪量程：
|
||||
|
||||
|
||||
|
||||
### 寄存器28 - 加速度计量程配置-ACCEL\_CONFIG
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
### 寄存器25 - 采样速率分频器-SMPRT\_DIV
|
||||
|
||||
该寄存器指定用于产生MPU-60X0采样率的陀螺仪输出速率的分频器。传感器寄存器输出，FIFO输出和DMP采样都基于采样率。采样率是通过将陀螺仪输出速率除以 SMPLRT\_DIV 产生的。
|
||||
|
||||
|
||||
|
||||
> Sample Rate = Gyroscope Output Rate / \(1 + SMPLRT\_DIV\) where Gyroscope Output Rate = 8kHz when the DLPF is disabled \(DLPF\_CFG = 0 or 7\), and 1kHz when the DLPF is enabled \(see Register 26\)
|
||||
|
||||
* 在不开启DLPF的情况下，设置 SMPLRT\_DIV 为 7 可以时芯片产生 1khz 的中断信号。
|
||||
|
||||
|
||||
|
||||
### 寄存器59到64-加速度计测量值
|
||||
|
||||
|
||||
|
||||
* 大数端存放：地址地位存放数据高位，地址高位存放数据地位。
|
||||
* 补码存放：因为测量值为有符号整数，因此采用补码方式存放
|
||||
|
||||
### 寄存器65和66 - 温度测量
|
||||
|
||||
|
||||
|
||||
### 寄存器67至72 - 陀螺仪测量值
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,15 @@
|
||||
|
||||
## 第三方代码
|
||||
|
||||
Additional third party copyrighted code is included under the following licenses.
|
||||
|
||||
| Component | License | Origin |commit id |
|
||||
| :---: | :---: | :---: |:---: |
|
||||
| core/crazyflie | GPL-3.0 |[Crazyflie](https://github.com/bitcraze/crazyflie-firmware) |a2a26abd53a5f328374877bfbcb7b25ed38d8111|
|
||||
| lib/dsp_lib | | [esp32-lin](https://github.com/whyengineer/esp32-lin/tree/master/components/dsp_lib) |6fa39f4cd5f7782b3a2a052767f0fb06be2378ff|
|
||||
|
||||
## 致谢
|
||||
|
||||
1. 感谢 Bitcraze 开源组织提供很棒的 [Crazyflie](https://www.bitcraze.io/%20)无人机项目代码
|
||||
2. 感谢 Espressif 提供 ESP32 和 [ESP-IDF操作系统](https://docs.espressif.com/projects/esp-idf/en/latest/esp32s2/get-started/index.html)
|
||||
3. 感谢 WhyEngineer 提供的 dsp 移植库 [esp-dsp](https://github.com/whyengineer/esp32-lin/tree/master/components/dsp_lib)
|
||||
@@ -1,33 +0,0 @@
|
||||
|
||||
## PID参数整定流程
|
||||
|
||||
**crazyflie `Rate PID`调整过程**
|
||||
|
||||
1. 先调整`Rate `模式，将`rollType `,`pitchType` 和 `yawType`都调整为`RATE`
|
||||
2. 将 `ATTITUDE`模式对应的 `roll`, `pitch` 和 `yaw`的`KP`,`KI`和`KD`调整为`0.0`，仅保留`Rate `相关的参数
|
||||
3. 将`RATE`模式对应的 `roll`, `pitch` 和 `yaw` 的`KI`和`KD`调整为`0.0`，先调整比例控制`KP`
|
||||
4. 烧写代码，使用cfclient的param功能开始在线进行`KP`的调整
|
||||
5. 注意，使用cfclient修改后的参数，掉电是不保存的。
|
||||
6. 注意安全，因为在PID调整期间会出现超调的情况
|
||||
7. 先固定住飞行器，让其只能进行`pitch`轴的翻转。逐渐增加`pitch`对应的`KP`,直到飞机出现前后的震荡（超调）
|
||||
8. 当出现严重的震荡时，可以稍微降低`KP`（ Once you reach the point of instability, tune it down 5-10 points），然后即可确定`KP`参数
|
||||
9. 同样的方法调整 `roll`
|
||||
10. 最后同样的方法调整`yaw`
|
||||
11. 下面调整 `KI`，该参数用于消除稳态误差，因为如果不引入该参数，只有比例调整的话，飞机受到重力等干扰会在0位置上线摆动。设置 `KI`的初始值为`KP`的50%。
|
||||
12. 当`KI`增大到一定程度，也会导致飞机不稳定的晃动，但是`KI`造成的晃动频率会相比`KP`带来的震动，频率更小。然后以造成这个状态的 `KI`为基础确定 `KI`的值（This is your critical KI, and so tune down 5-10 points.）
|
||||
13. 同样的方法调整 `roll` 和 `yaw`
|
||||
14. yaw axis, except KI is usually around 80%+ of KP.
|
||||
|
||||
****
|
||||
以上完成了`Rate `模式参数的调整
|
||||
****
|
||||
|
||||
**下面开始整定 `Attitude PID`**
|
||||
|
||||
14. 确保`Rate PID`调整已经完成。
|
||||
15. 将`rollType `,`pitchType` 和 `yawType`都调整为`ANGLE`，意味着飞机已经进入attitude mode。
|
||||
16. 改变 `roll`和`pitch`的`KI`和`KD`为`0.0`，将`Yaw` 的 `KP``KI``KD`都设置为`0.0` 。
|
||||
17. 烧写代码，使用cfclient的param功能开始在线进行`KP`的调整。
|
||||
18. 将`roll`和`pitch`的`KP `设置为`3.5`，寻找任何不稳定性，例如振荡。持续增加KP，直到达到极限。
|
||||
19. 如果您发现`KP`导致不稳定，如果此时已经高于`4`，需要将`RATE`模式的 `KP`和`KI`稍微降低5-10点。这使您在调整姿势模式时更加“自由”
|
||||
20. 要调整KI，请再次缓慢增加KI。不稳定性的状态是产生低频振荡。
|
||||
@@ -1,136 +0,0 @@
|
||||
|
||||
## PMW3901传感器特性
|
||||
|
||||
PMW3901 是 PixArt 公司最新的高精度低功耗光学追踪模组，可直接获取 xy 方向运动速度信息，对地高度 8cm 以上实现有效测量 ，工作电流 < 9mA ， 工作电压 VDD(1.8~2.1VDC) VDDIO(1.8~3.6VDC)，使用 4 线 SPI 接口通信。
|
||||
|
||||
**主要参数**
|
||||
|
||||
| Parameter | Value |
|
||||
|--|--|
|
||||
| Supply Voltage (V) | VDD: 1.8 – 2.1 VDDIO: 1.8 – 3.6 |
|
||||
|Working Range (mm) | 80 to infinity |
|
||||
| Interface | 4-Wire SPI @ 2 Mhz|
|
||||
| Package Type | 28-pin COB Package with Lens Assembly:6 x 6 x 2.28 mm |
|
||||
|
||||
**封装和引脚图**
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
> 传感器工作电压较低，与3.3V的ＥＳＰ３２通信，需要ＶＤＤ和ＶＤＤＩＯ提供不同的电压
|
||||
|
||||
## Power States & Sequence
|
||||
|
||||
**Power-Up Sequence**
|
||||
Although PMW3901MB performs an internal power up self-reset, it is still recommended that the Power_Up_Reset register is written every time power is applied. The appropriate sequence is as follows:
|
||||
1. Apply power to VDDIO first and followed by VDD, with a delay of no more than 100ms in between each supply. Ensure all supplies are stable.
|
||||
2. Wait for at least 40 ms.
|
||||
3. Drive NCS high, and then low to reset the SPI port.
|
||||
4. Write 0x5A to Power_Up_Reset register (or alternatively, toggle the NRESET pin).
|
||||
5. Wait for at least 1 ms.
|
||||
6. Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion pin state.
|
||||
7. Refer Section 8.2 Performance Optimization Registers to configure the needed registers in order to achieve optimum performance of the chip.
|
||||
|
||||
|
||||
**Power-Down Sequence**
|
||||
PMW3901MB can be set to Shutdown mode by writing to Shutdown register. The SPI port should not be accessed when Shutdown mode is asserted, except the power-up command (writing 0x5A to register 0x3A). Other ICs on the same SPI bus can be accessed, as long as the chip’s NCS pin is not asserted.
|
||||
To de-assert Shutdown mode:
|
||||
8. Drive NCS high, and then low to reset the SPI port.
|
||||
9. Write 0x5A to Power_Up_Reset register (or alternatively, toggle the NRESET pin).
|
||||
10. Wait for at least 1 ms.
|
||||
11. Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion pin state.
|
||||
12. Refer Section 8.2 Performance Optimization Registers to configure the needed registers in order to achieve optimum performance of the chip.
|
||||
|
||||
|
||||
[pixart其他产品助力IOT](https://www.pixart.com/applications/11/Connected_Home_Appliances_%EF%BC%86_IoT)
|
||||
|
||||
## 与飞控程序通信
|
||||
|
||||
### 关键结构体
|
||||
|
||||
```
|
||||
|
||||
typedef struct opFlow_s
|
||||
{
|
||||
float pixSum[2]; /*累积像素*/
|
||||
float pixComp[2]; /*像素补偿*/
|
||||
float pixValid[2]; /*有效像素*/
|
||||
float pixValidLast[2]; /*上一次有效像素*/
|
||||
float deltaPos[2]; /*2 帧之间的位移 单位 cm*/
|
||||
float deltaVel[2]; /*速度 单位 cm/s*/
|
||||
float posSum[2]; /*累积位移 单位 cm*/
|
||||
float velLpf[2]; /*速度低通 单位 cm/s*/
|
||||
bool isOpFlowOk; /*光流状态*/
|
||||
bool isDataValid; /*数据有效*/
|
||||
} opFlow_t;
|
||||
|
||||
```
|
||||
|
||||
* 累积像素，就是自四轴起飞后的累积像素；
|
||||
* 像素补偿，就是补偿由于飞机倾斜导致的像素误差；
|
||||
* 有效像素，指经过补偿的实际像素；
|
||||
* 2 帧之间的位移，这个就是由像素转换出来的实际位移，单位 cm；
|
||||
* 速度，这个速度是瞬时速度，由位移变化量微分得到，单位 cm/s；
|
||||
* 累积位移，实际位移，单位 cm速度低通，对速度进行低通，增加数据平滑性；
|
||||
* 光流状态，光流是否正常工作；
|
||||
* 数据有效，在一定高度范围内，数据有效；
|
||||
|
||||
```
|
||||
typedef struct motionBurst_s {
|
||||
union {
|
||||
uint8_t motion;
|
||||
struct {
|
||||
uint8_t frameFrom0 : 1;
|
||||
uint8_t runMode : 2;
|
||||
uint8_t reserved1 : 1;
|
||||
uint8_t rawFrom0 : 1;
|
||||
uint8_t reserved2 : 2;
|
||||
uint8_t motionOccured : 1;
|
||||
};
|
||||
};
|
||||
|
||||
uint8_t observation;
|
||||
int16_t deltaX;
|
||||
int16_t deltaY;
|
||||
|
||||
uint8_t squal;
|
||||
|
||||
uint8_t rawDataSum;
|
||||
uint8_t maxRawData;
|
||||
uint8_t minRawData;
|
||||
|
||||
uint16_t shutter;
|
||||
} __attribute__((packed)) motionBurst_t;
|
||||
```
|
||||
|
||||
* motion ：运动信息，可以根据不同的位去判断运动信息，包括帧判别，运行模式和运动信息检测等。
|
||||
* observation：这个是用于检测 IC 是否出现 EFT/B 或者 ESD 问题，传感器正常工作时，读取出来的值为0xBF。
|
||||
* deltaX, deltaY ：光流检测到图像的 X 和 Y 方向的运动信息。
|
||||
* squal ：指运动信息质量，简单说就是运动信息的可信度。
|
||||
* rawDataSum ：这个是原数据求和，可用作对一帧数据求平均值；maxRawData 和 minRawData ，是最大和最小原始数据；
|
||||
* shutter：是一个实时自动调整的值，目的是保证平均运动数据在正常可操作范围以内，这个值可以搭配 squal，用来判断运动信息是否可用。
|
||||
|
||||
|
||||
### 编程注意事项
|
||||
|
||||
1. 如果连续 1s 内光流数据都为 0，说明出现故障，需要做挂起光流任务等处理。
|
||||
2. 可以设置pitch为x，roll方向为y，需要注意因为传感器向下安装，且地面不动，**飞机向前走，图像是向后的。**
|
||||
3. 需要测量准确的高度，同于确定图像像素和实际距离的对应关系，这个高度会作为一个计算的参数（因此只有在定高模式稳定才能定点）光流手册 42 页计算关系。
|
||||
|
||||
```
|
||||
补充计算关系与代码实现
|
||||
```
|
||||
|
||||
4. 需要手动测试倾角补偿，效果是，能够通过补偿，使飞行器有一定的倾角时，传感器输出基本不变化
|
||||
|
||||
```
|
||||
补充测试过程
|
||||
```
|
||||
|
||||
5. 有了倾角补偿和运动累积像素，我们就可以得到实际累积像素，减去上次的实际像素，就可以得到 2 帧之间的变化像素，再乘以系数就可以得到 2 帧之间的位移变化，可以看到还有对系数的限制，当高度小于 5cm，光流就无法工作了，所以系数设置为 0。接着对这个位移积分得到四轴到起飞点的位移，对这个位移微分得到瞬时速度，对速度进行低通增加数据的平滑性，对速度进行限幅处理，增加数据安全性。
|
||||
6. 通过光流就得到了四轴的位置信息和速度信息，把这些位置信息和速度信息融合加速计（state_estimator.c），得到估测位置和速度，将估测位置和速度参与 PID 运算，即可用于水平方向位置控制，这部分内容请看 position_pid.c，源码里面可以直接看到位置环和速度环 PID 的处理过程，这样就可以实现水平定点控制了。
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,216 +0,0 @@
|
||||
|
||||
## 传感器驱动
|
||||
|
||||
传感器驱动代码，可以在 `components\drivers` 中查阅，`drivers` 使用了与 [esp-iot-solution](https://github.com/espressif/esp-iot-solution/)类似的文件结构，将驱动程序按照所属总线进行分类，包括 `i2c_devices` 、`spi_devices`、`general` 等。具体可参考：[drivers](./drivers)
|
||||
|
||||
|
||||
|
||||
## 传感器硬件抽象层
|
||||
|
||||
`components\core\crazyflie\hal\src\sensors.c` 文件对传感器进行了硬件抽象，开发者可以自由组合传感器，通过实现硬件抽象层定义的传感器接口，与上层应用进行对接。
|
||||
|
||||
```
|
||||
typedef struct {
|
||||
SensorImplementation_t implements;
|
||||
void (*init)(void);
|
||||
bool (*test)(void);
|
||||
bool (*areCalibrated)(void);
|
||||
bool (*manufacturingTest)(void);
|
||||
void (*acquire)(sensorData_t *sensors, const uint32_t tick);
|
||||
void (*waitDataReady)(void);
|
||||
bool (*readGyro)(Axis3f *gyro);
|
||||
bool (*readAcc)(Axis3f *acc);
|
||||
bool (*readMag)(Axis3f *mag);
|
||||
bool (*readBaro)(baro_t *baro);
|
||||
void (*setAccMode)(accModes accMode);
|
||||
void (*dataAvailableCallback)(void);
|
||||
} sensorsImplementation_t;
|
||||
```
|
||||
|
||||
esp-drone 实现的传感器抽象接口在 `components/core/crazyflie/hal/src/sensors_mpu6050_hm5883L_ms5611.c` 中，通过以下赋值过程与上层应用对接：
|
||||
|
||||
```
|
||||
#ifdef SENSOR_INCLUDED_MPU6050_HMC5883L_MS5611
|
||||
{
|
||||
.implements = SensorImplementation_mpu6050_HMC5883L_MS5611,
|
||||
.init = sensorsMpu6050Hmc5883lMs5611Init,
|
||||
.test = sensorsMpu6050Hmc5883lMs5611Test,
|
||||
.areCalibrated = sensorsMpu6050Hmc5883lMs5611AreCalibrated,
|
||||
.manufacturingTest = sensorsMpu6050Hmc5883lMs5611ManufacturingTest,
|
||||
.acquire = sensorsMpu6050Hmc5883lMs5611Acquire,
|
||||
.waitDataReady = sensorsMpu6050Hmc5883lMs5611WaitDataReady,
|
||||
.readGyro = sensorsMpu6050Hmc5883lMs5611ReadGyro,
|
||||
.readAcc = sensorsMpu6050Hmc5883lMs5611ReadAcc,
|
||||
.readMag = sensorsMpu6050Hmc5883lMs5611ReadMag,
|
||||
.readBaro = sensorsMpu6050Hmc5883lMs5611ReadBaro,
|
||||
.setAccMode = sensorsMpu6050Hmc5883lMs5611SetAccMode,
|
||||
.dataAvailableCallback = nullFunction,
|
||||
}
|
||||
#endif
|
||||
```
|
||||
|
||||
## 传感器校准过程
|
||||
|
||||
### 陀螺仪校准过程
|
||||
|
||||
由于陀螺仪存在较大的温漂，因此每次使用前需要对陀螺仪进行校准，计算当前环境下的陀螺仪基准值。ESP-Drone 延续 Crazyflie2 陀螺仪校准方案，在初次上电时，计算陀螺仪三个轴的方差与平均值。
|
||||
|
||||
1. 使用一个最大长度为 1024 的环形缓冲区，存储最新的 1024 组陀螺仪测量值
|
||||
2. 通过计算陀螺仪输出值方差，确认飞机已经放置平稳并且陀螺仪工作正常。
|
||||
3. 确认第 2 步正常后，计算静止时 1024 组陀螺仪输出值的平均值，作为陀螺仪的校准值
|
||||
|
||||
|
||||
**陀螺仪基准值计算源代码：**
|
||||
|
||||
```
|
||||
/**
|
||||
* Adds a new value to the variance buffer and if it is full
|
||||
* replaces the oldest one. Thus a circular buffer.
|
||||
*/
|
||||
static void sensorsAddBiasValue(BiasObj* bias, int16_t x, int16_t y, int16_t z)
|
||||
{
|
||||
bias->bufHead->x = x;
|
||||
bias->bufHead->y = y;
|
||||
bias->bufHead->z = z;
|
||||
bias->bufHead++;
|
||||
|
||||
if (bias->bufHead >= &bias->buffer[SENSORS_NBR_OF_BIAS_SAMPLES])
|
||||
{
|
||||
bias->bufHead = bias->buffer;
|
||||
bias->isBufferFilled = true;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if the variances is below the predefined thresholds.
|
||||
* The bias value should have been added before calling this.
|
||||
* @param bias The bias object
|
||||
*/
|
||||
static bool sensorsFindBiasValue(BiasObj* bias)
|
||||
{
|
||||
static int32_t varianceSampleTime;
|
||||
bool foundBias = false;
|
||||
|
||||
if (bias->isBufferFilled)
|
||||
{
|
||||
sensorsCalculateVarianceAndMean(bias, &bias->variance, &bias->mean);
|
||||
|
||||
if (bias->variance.x < GYRO_VARIANCE_THRESHOLD_X &&
|
||||
bias->variance.y < GYRO_VARIANCE_THRESHOLD_Y &&
|
||||
bias->variance.z < GYRO_VARIANCE_THRESHOLD_Z &&
|
||||
(varianceSampleTime + GYRO_MIN_BIAS_TIMEOUT_MS < xTaskGetTickCount()))
|
||||
{
|
||||
varianceSampleTime = xTaskGetTickCount();
|
||||
bias->bias.x = bias->mean.x;
|
||||
bias->bias.y = bias->mean.y;
|
||||
bias->bias.z = bias->mean.z;
|
||||
foundBias = true;
|
||||
bias->isBiasValueFound = true;
|
||||
}
|
||||
}
|
||||
|
||||
return foundBias;
|
||||
}
|
||||
```
|
||||
|
||||
**修正陀螺仪输出值：**
|
||||
|
||||
```
|
||||
sensorData.gyro.x = (gyroRaw.x - gyroBias.x) * SENSORS_DEG_PER_LSB_CFG;
|
||||
sensorData.gyro.y = (gyroRaw.y - gyroBias.y) * SENSORS_DEG_PER_LSB_CFG;
|
||||
sensorData.gyro.z = (gyroRaw.z - gyroBias.z) * SENSORS_DEG_PER_LSB_CFG;
|
||||
applyAxis3fLpf((lpf2pData *)(&gyroLpf), &sensorData.gyro); //低通滤波器，去除高频干扰
|
||||
```
|
||||
|
||||
|
||||
### 加速度计校准过程
|
||||
|
||||
#### 重力加速度校准
|
||||
|
||||
在地球不同的纬度和海拔下，重力加速度 g 值一般不同，因此需要使用加速度计对 g 进行实际测量。参考 Crazyflie2 加速度计校准方案，g 值的校准过程如下：
|
||||
|
||||
1. 陀螺仪校准完成后，立刻进行加速度计校准。
|
||||
2. 使用 buffer 保存 200 组加速度计测量值
|
||||
3. 通过合成重力加速度在三个轴的分量，计算重力加速度在静止状态下的值。
|
||||
|
||||
参考：[不同地球纬度和海拔下的不同重力加速度值 g](https://baike.baidu.com/item/%E9%87%8D%E5%8A%9B%E5%8A%A0%E9%80%9F%E5%BA%A6/23553)
|
||||
|
||||
**计算静止状态下重力加速度值：**
|
||||
|
||||
```
|
||||
/**
|
||||
* Calculates accelerometer scale out of SENSORS_ACC_SCALE_SAMPLES samples. Should be called when
|
||||
* platform is stable.
|
||||
*/
|
||||
static bool processAccScale(int16_t ax, int16_t ay, int16_t az)
|
||||
{
|
||||
static bool accBiasFound = false;
|
||||
static uint32_t accScaleSumCount = 0;
|
||||
|
||||
if (!accBiasFound)
|
||||
{
|
||||
accScaleSum += sqrtf(powf(ax * SENSORS_G_PER_LSB_CFG, 2) + powf(ay * SENSORS_G_PER_LSB_CFG, 2) + powf(az * SENSORS_G_PER_LSB_CFG, 2));
|
||||
accScaleSumCount++;
|
||||
|
||||
if (accScaleSumCount == SENSORS_ACC_SCALE_SAMPLES)
|
||||
{
|
||||
accScale = accScaleSum / SENSORS_ACC_SCALE_SAMPLES;
|
||||
accBiasFound = true;
|
||||
}
|
||||
}
|
||||
|
||||
return accBiasFound;
|
||||
}
|
||||
```
|
||||
|
||||
**通过实际重力加速度值，修正加速度计测量值：**
|
||||
|
||||
```
|
||||
accScaled.x = (accelRaw.x) * SENSORS_G_PER_LSB_CFG / accScale;
|
||||
accScaled.y = (accelRaw.y) * SENSORS_G_PER_LSB_CFG / accScale;
|
||||
accScaled.z = (accelRaw.z) * SENSORS_G_PER_LSB_CFG / accScale;
|
||||
```
|
||||
|
||||
|
||||
#### 机身水平校准
|
||||
|
||||
理想状态下，加速度传感器在飞机上完全水平的进行安装，进而可以使用 0 位置作为飞机的水平面，但是由于加速度计在安装时不可避免的存在一定的倾角，导致飞控错误的估计水平位置，导致飞机向某个方向偏飞。因此需要设置一定的校准策略来平衡这种误差。
|
||||
|
||||
1. 将飞机放置在一个水平面上，计算飞机 `cosRoll` `sinRoll` `cosPitch` `sinPitch` 。理想状态下 `cosRoll` `cosPitch` 为 1 ，`sinPitch` `sinRoll` 为 0 。如果不是水平安装`sinPitch` `sinRoll` 不为 0，`cosRoll` `cosPitch` 不为 1 。
|
||||
2. 将步骤 1 的 `cosRoll` `sinRoll` `cosPitch` `sinPitch` 或对应的 `Roll` `Pitch` 角度值保存到飞机，用于校准。
|
||||
|
||||
|
||||
**利用校准值，对加速度计测量值进行修正：**
|
||||
|
||||
```
|
||||
/**
|
||||
* Compensate for a miss-aligned accelerometer. It uses the trim
|
||||
* data gathered from the UI and written in the config-block to
|
||||
* rotate the accelerometer to be aligned with gravity.
|
||||
*/
|
||||
static void sensorsAccAlignToGravity(Axis3f *in, Axis3f *out)
|
||||
{
|
||||
//TODO: need cosPitch calculate firstly
|
||||
Axis3f rx;
|
||||
Axis3f ry;
|
||||
|
||||
// Rotate around x-axis
|
||||
rx.x = in->x;
|
||||
rx.y = in->y * cosRoll - in->z * sinRoll;
|
||||
rx.z = in->y * sinRoll + in->z * cosRoll;
|
||||
|
||||
// Rotate around y-axis
|
||||
ry.x = rx.x * cosPitch - rx.z * sinPitch;
|
||||
ry.y = rx.y;
|
||||
ry.z = -rx.x * sinPitch + rx.z * cosPitch;
|
||||
|
||||
out->x = ry.x;
|
||||
out->y = ry.y;
|
||||
out->z = ry.z;
|
||||
}
|
||||
```
|
||||
|
||||
以上过程，可通过力的分解和勾股定理推导。
|
||||
|
||||
|
||||
|
||||
+425
-10
@@ -1,22 +1,24 @@
|
||||
|
||||
# 飞控系统
|
||||
|
||||
## 系统启动流程
|
||||
|
||||
|
||||
|
||||
查阅源文件：[start_from_app_main](../../_static/start_from_app_main.pdf)
|
||||
查阅源文件：[start_from_app_main](./_static/start_from_app_main.pdf)
|
||||
|
||||
## 系统任务管理
|
||||
|
||||
### 系统任务简介
|
||||
|
||||
**系统正常运行时，将启动以下 TASK：**
|
||||
|
||||
|
||||
|
||||
* Load: CPU 占用率
|
||||
* Stack Left：剩余堆栈空间
|
||||
* Name：TASK 名称
|
||||
* PRI: TASK 优先级
|
||||
|
||||
**系统任务简介：**
|
||||
> Load: CPU 占用率
|
||||
> Stack Left：剩余堆栈空间
|
||||
> Name：TASK 名称
|
||||
> PRI: TASK 优先级
|
||||
|
||||
* PWRMGNT: 系统电压监测
|
||||
* CMDHL: 应用层-处理根据 CRTP 协议构成的高级命令
|
||||
@@ -33,7 +35,7 @@
|
||||
* STABILIZER: 自稳定线程，控制飞控程序运行流程
|
||||
* SYSTEM: 控制系统初始化和自检流程
|
||||
|
||||
**TASK 堆栈空间配置**
|
||||
### 任务堆栈空间配置
|
||||
|
||||
可以在 `components/config/include/config.h` 中直接修改空间大小，也可以在 `menucfg` 中修改 `BASE_STACK_SIZE` 大小。使用 `ESP32` 时可将 `BASE_STACK_SIZE` 调整为 2048，减小踩空间的概率，使用 `ESP32S2` 时，建议将该值调整为 `1024`。
|
||||
|
||||
@@ -75,7 +77,7 @@
|
||||
```
|
||||
|
||||
|
||||
**TASK 优先级配置**
|
||||
### 任务优先级配置
|
||||
|
||||
系统 TASK 优先级可以在 `components/config/include/config.h` 中进行配置，由于 `ESP32` 具有双核优势，相比 `ESP32S2` 计算资源更加富余，可将高耗时的 `KALMAN_TASK` 优先级调高。 在使用 `ESP32S2` 时，需要将高耗时的 `KALMAN_TASK` 优先级调低，否者难以释放足够的 CPU 资源，将触发 task watchdog。
|
||||
|
||||
@@ -129,10 +131,423 @@
|
||||
#define UDP_RX2_TASK_PRI 3
|
||||
```
|
||||
|
||||
## 关键 TASK 介绍
|
||||
## 关键任务介绍
|
||||
|
||||
除了系统默认开启的 task（如 wifi task），优先级最高的 task 是 `STABILIZER_TASK`，凸显了这个任务的重要性。`STABILIZER_TASK` 控制了从传感器数据读取，到姿态计算，到目标接收，到最终输出电机功率的整个过程，驱动各个阶段的算法运行。
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
## 传感器驱动
|
||||
|
||||
传感器驱动代码，可以在 `components\drivers` 中查阅，`drivers` 使用了与 [esp-iot-solution](https://github.com/espressif/esp-iot-solution/)类似的文件结构，将驱动程序按照所属总线进行分类，包括 `i2c_devices` 、`spi_devices`、`general` 等。具体可参考：[drivers](./drivers)
|
||||
|
||||
|
||||
|
||||
## 传感器硬件抽象
|
||||
|
||||
`components\core\crazyflie\hal\src\sensors.c` 文件对传感器进行了硬件抽象，开发者可以自由组合传感器，通过实现硬件抽象层定义的传感器接口，与上层应用进行对接。
|
||||
|
||||
```
|
||||
typedef struct {
|
||||
SensorImplementation_t implements;
|
||||
void (*init)(void);
|
||||
bool (*test)(void);
|
||||
bool (*areCalibrated)(void);
|
||||
bool (*manufacturingTest)(void);
|
||||
void (*acquire)(sensorData_t *sensors, const uint32_t tick);
|
||||
void (*waitDataReady)(void);
|
||||
bool (*readGyro)(Axis3f *gyro);
|
||||
bool (*readAcc)(Axis3f *acc);
|
||||
bool (*readMag)(Axis3f *mag);
|
||||
bool (*readBaro)(baro_t *baro);
|
||||
void (*setAccMode)(accModes accMode);
|
||||
void (*dataAvailableCallback)(void);
|
||||
} sensorsImplementation_t;
|
||||
```
|
||||
|
||||
esp-drone 实现的传感器抽象接口在 `components/core/crazyflie/hal/src/sensors_mpu6050_hm5883L_ms5611.c` 中，通过以下赋值过程与上层应用对接：
|
||||
|
||||
```
|
||||
#ifdef SENSOR_INCLUDED_MPU6050_HMC5883L_MS5611
|
||||
{
|
||||
.implements = SensorImplementation_mpu6050_HMC5883L_MS5611,
|
||||
.init = sensorsMpu6050Hmc5883lMs5611Init,
|
||||
.test = sensorsMpu6050Hmc5883lMs5611Test,
|
||||
.areCalibrated = sensorsMpu6050Hmc5883lMs5611AreCalibrated,
|
||||
.manufacturingTest = sensorsMpu6050Hmc5883lMs5611ManufacturingTest,
|
||||
.acquire = sensorsMpu6050Hmc5883lMs5611Acquire,
|
||||
.waitDataReady = sensorsMpu6050Hmc5883lMs5611WaitDataReady,
|
||||
.readGyro = sensorsMpu6050Hmc5883lMs5611ReadGyro,
|
||||
.readAcc = sensorsMpu6050Hmc5883lMs5611ReadAcc,
|
||||
.readMag = sensorsMpu6050Hmc5883lMs5611ReadMag,
|
||||
.readBaro = sensorsMpu6050Hmc5883lMs5611ReadBaro,
|
||||
.setAccMode = sensorsMpu6050Hmc5883lMs5611SetAccMode,
|
||||
.dataAvailableCallback = nullFunction,
|
||||
}
|
||||
#endif
|
||||
```
|
||||
|
||||
## 传感器校准过程
|
||||
|
||||
### 陀螺仪校准过程
|
||||
|
||||
由于陀螺仪存在较大的温漂，因此每次使用前需要对陀螺仪进行校准，计算当前环境下的陀螺仪基准值。ESP-Drone 延续 Crazyflie2 陀螺仪校准方案，在初次上电时，计算陀螺仪三个轴的方差与平均值。
|
||||
|
||||
1. 使用一个最大长度为 1024 的环形缓冲区，存储最新的 1024 组陀螺仪测量值
|
||||
2. 通过计算陀螺仪输出值方差，确认飞机已经放置平稳并且陀螺仪工作正常。
|
||||
3. 确认第 2 步正常后，计算静止时 1024 组陀螺仪输出值的平均值，作为陀螺仪的校准值
|
||||
|
||||
|
||||
**陀螺仪基准值计算源代码：**
|
||||
|
||||
```
|
||||
/**
|
||||
* Adds a new value to the variance buffer and if it is full
|
||||
* replaces the oldest one. Thus a circular buffer.
|
||||
*/
|
||||
static void sensorsAddBiasValue(BiasObj* bias, int16_t x, int16_t y, int16_t z)
|
||||
{
|
||||
bias->bufHead->x = x;
|
||||
bias->bufHead->y = y;
|
||||
bias->bufHead->z = z;
|
||||
bias->bufHead++;
|
||||
|
||||
if (bias->bufHead >= &bias->buffer[SENSORS_NBR_OF_BIAS_SAMPLES])
|
||||
{
|
||||
bias->bufHead = bias->buffer;
|
||||
bias->isBufferFilled = true;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if the variances is below the predefined thresholds.
|
||||
* The bias value should have been added before calling this.
|
||||
* @param bias The bias object
|
||||
*/
|
||||
static bool sensorsFindBiasValue(BiasObj* bias)
|
||||
{
|
||||
static int32_t varianceSampleTime;
|
||||
bool foundBias = false;
|
||||
|
||||
if (bias->isBufferFilled)
|
||||
{
|
||||
sensorsCalculateVarianceAndMean(bias, &bias->variance, &bias->mean);
|
||||
|
||||
if (bias->variance.x < GYRO_VARIANCE_THRESHOLD_X &&
|
||||
bias->variance.y < GYRO_VARIANCE_THRESHOLD_Y &&
|
||||
bias->variance.z < GYRO_VARIANCE_THRESHOLD_Z &&
|
||||
(varianceSampleTime + GYRO_MIN_BIAS_TIMEOUT_MS < xTaskGetTickCount()))
|
||||
{
|
||||
varianceSampleTime = xTaskGetTickCount();
|
||||
bias->bias.x = bias->mean.x;
|
||||
bias->bias.y = bias->mean.y;
|
||||
bias->bias.z = bias->mean.z;
|
||||
foundBias = true;
|
||||
bias->isBiasValueFound = true;
|
||||
}
|
||||
}
|
||||
|
||||
return foundBias;
|
||||
}
|
||||
```
|
||||
|
||||
**修正陀螺仪输出值：**
|
||||
|
||||
```
|
||||
sensorData.gyro.x = (gyroRaw.x - gyroBias.x) * SENSORS_DEG_PER_LSB_CFG;
|
||||
sensorData.gyro.y = (gyroRaw.y - gyroBias.y) * SENSORS_DEG_PER_LSB_CFG;
|
||||
sensorData.gyro.z = (gyroRaw.z - gyroBias.z) * SENSORS_DEG_PER_LSB_CFG;
|
||||
applyAxis3fLpf((lpf2pData *)(&gyroLpf), &sensorData.gyro); //低通滤波器，去除高频干扰
|
||||
```
|
||||
|
||||
|
||||
### 加速度计校准过程
|
||||
|
||||
#### 重力加速度校准
|
||||
|
||||
在地球不同的纬度和海拔下，重力加速度 g 值一般不同，因此需要使用加速度计对 g 进行实际测量。参考 Crazyflie2 加速度计校准方案，g 值的校准过程如下：
|
||||
|
||||
1. 陀螺仪校准完成后，立刻进行加速度计校准。
|
||||
2. 使用 buffer 保存 200 组加速度计测量值
|
||||
3. 通过合成重力加速度在三个轴的分量，计算重力加速度在静止状态下的值。
|
||||
|
||||
参考：[不同地球纬度和海拔下的不同重力加速度值 g](https://baike.baidu.com/item/%E9%87%8D%E5%8A%9B%E5%8A%A0%E9%80%9F%E5%BA%A6/23553)
|
||||
|
||||
**计算静止状态下重力加速度值：**
|
||||
|
||||
```
|
||||
/**
|
||||
* Calculates accelerometer scale out of SENSORS_ACC_SCALE_SAMPLES samples. Should be called when
|
||||
* platform is stable.
|
||||
*/
|
||||
static bool processAccScale(int16_t ax, int16_t ay, int16_t az)
|
||||
{
|
||||
static bool accBiasFound = false;
|
||||
static uint32_t accScaleSumCount = 0;
|
||||
|
||||
if (!accBiasFound)
|
||||
{
|
||||
accScaleSum += sqrtf(powf(ax * SENSORS_G_PER_LSB_CFG, 2) + powf(ay * SENSORS_G_PER_LSB_CFG, 2) + powf(az * SENSORS_G_PER_LSB_CFG, 2));
|
||||
accScaleSumCount++;
|
||||
|
||||
if (accScaleSumCount == SENSORS_ACC_SCALE_SAMPLES)
|
||||
{
|
||||
accScale = accScaleSum / SENSORS_ACC_SCALE_SAMPLES;
|
||||
accBiasFound = true;
|
||||
}
|
||||
}
|
||||
|
||||
return accBiasFound;
|
||||
}
|
||||
```
|
||||
|
||||
**通过实际重力加速度值，修正加速度计测量值：**
|
||||
|
||||
```
|
||||
accScaled.x = (accelRaw.x) * SENSORS_G_PER_LSB_CFG / accScale;
|
||||
accScaled.y = (accelRaw.y) * SENSORS_G_PER_LSB_CFG / accScale;
|
||||
accScaled.z = (accelRaw.z) * SENSORS_G_PER_LSB_CFG / accScale;
|
||||
```
|
||||
|
||||
|
||||
#### 机身水平校准
|
||||
|
||||
理想状态下，加速度传感器在飞机上完全水平的进行安装，进而可以使用 0 位置作为飞机的水平面，但是由于加速度计在安装时不可避免的存在一定的倾角，导致飞控错误的估计水平位置，导致飞机向某个方向偏飞。因此需要设置一定的校准策略来平衡这种误差。
|
||||
|
||||
1. 将飞机放置在一个水平面上，计算飞机 `cosRoll` `sinRoll` `cosPitch` `sinPitch` 。理想状态下 `cosRoll` `cosPitch` 为 1 ，`sinPitch` `sinRoll` 为 0 。如果不是水平安装`sinPitch` `sinRoll` 不为 0，`cosRoll` `cosPitch` 不为 1 。
|
||||
2. 将步骤 1 的 `cosRoll` `sinRoll` `cosPitch` `sinPitch` 或对应的 `Roll` `Pitch` 角度值保存到飞机，用于校准。
|
||||
|
||||
|
||||
**利用校准值，对加速度计测量值进行修正：**
|
||||
|
||||
```
|
||||
/**
|
||||
* Compensate for a miss-aligned accelerometer. It uses the trim
|
||||
* data gathered from the UI and written in the config-block to
|
||||
* rotate the accelerometer to be aligned with gravity.
|
||||
*/
|
||||
static void sensorsAccAlignToGravity(Axis3f *in, Axis3f *out)
|
||||
{
|
||||
//TODO: need cosPitch calculate firstly
|
||||
Axis3f rx;
|
||||
Axis3f ry;
|
||||
|
||||
// Rotate around x-axis
|
||||
rx.x = in->x;
|
||||
rx.y = in->y * cosRoll - in->z * sinRoll;
|
||||
rx.z = in->y * sinRoll + in->z * cosRoll;
|
||||
|
||||
// Rotate around y-axis
|
||||
ry.x = rx.x * cosPitch - rx.z * sinPitch;
|
||||
ry.y = rx.y;
|
||||
ry.z = -rx.x * sinPitch + rx.z * cosPitch;
|
||||
|
||||
out->x = ry.x;
|
||||
out->y = ry.y;
|
||||
out->z = ry.z;
|
||||
}
|
||||
```
|
||||
|
||||
以上过程，可通过力的分解和勾股定理推导。
|
||||
|
||||
## 姿态计算
|
||||
|
||||
### 已经支持的姿态计算算法
|
||||
|
||||
* 互补滤波
|
||||
* 卡尔曼滤波
|
||||
|
||||
ESP-Drone 姿态计算代码来自 `crazyflie`，ESP-Drone 固件已经对互补滤波和卡尔曼滤波进行了实际测试，可以有效的计算飞行姿态，包括各个自由度的角度、角速度、和空间位置，为控制系统提供了可靠的状态输入。需要注意的是，在定点模式下，必须切换到卡尔曼滤波算法，才能保证工作正常。
|
||||
|
||||
crazyflie 状态估计：https://www.bitcraze.io/2020/01/state-estimation-to-be-or-not-to-be/
|
||||
|
||||
### 互补滤波
|
||||
|
||||
|
||||
|
||||
互补滤波中文说明可参考：https://zhuanlan.zhihu.com/p/34323865
|
||||
|
||||
### 卡尔曼滤波
|
||||
|
||||
|
||||
|
||||
卡尔曼滤波中文说明可参考：https://zhuanlan.zhihu.com/p/39912633
|
||||
|
||||
## 控制算法
|
||||
|
||||
### 已支持的控制器
|
||||
|
||||
ESP-Drone 控制系统代码来自 `crazyflie`，也继承了该工程的所有控制算法，需要注意的是，ESP-Drone 仅对 PID 控制器进行了参数整定和测试，换用其它控制器时，请在确保安全的情况下，自行进行参数整定。
|
||||
|
||||
|
||||
|
||||
详情请参考：[https://www.bitcraze.io/2020/02/out-of-control/](https://www.bitcraze.io/2020/02/out-of-control/)
|
||||
|
||||
可在代码中,通过修改 `controllerInit(ControllerType controller)` 的传入参数，对控制器进行切换。
|
||||
|
||||
也可通过实现以下控制器接口，添加自定义的控制器：
|
||||
|
||||
```
|
||||
static ControllerFcns controllerFunctions[] = {
|
||||
{.init = 0, .test = 0, .update = 0, .name = "None"}, // Any
|
||||
{.init = controllerPidInit, .test = controllerPidTest, .update = controllerPid, .name = "PID"},
|
||||
{.init = controllerMellingerInit, .test = controllerMellingerTest, .update = controllerMellinger, .name = "Mellinger"},
|
||||
{.init = controllerINDIInit, .test = controllerINDITest, .update = controllerINDI, .name = "INDI"},
|
||||
};
|
||||
```
|
||||
|
||||
### PID控制器
|
||||
|
||||
**控制原理**
|
||||
|
||||
PID 控制器（比例-积分-微分控制器），由比例单元（Proportional）、积分单元（Integral）和微分单元（Derivative）组成，分别对应当前误差、过去累计误差及未来误差，最终基于误差和误差的变化率对系统进行控制。PID 控制器由于具有负反馈修正作用，一般被认为是最适用的控制器。通过调整 PID 控制器的三类参数，可以调整系统对误差的反应快慢、控制器过冲的程度及系统震荡的程度，使系统达到最优状态。
|
||||
|
||||
在飞行器系统中，由于存在 `pitch` 、 `roll` 、 `yaw` 三个自由度，因此需要设计如下图所示的具有控制闭环的 PID 控制器。
|
||||
|
||||
|
||||
|
||||
> 图片来自 Sun Feb 08, 2015
|
||||
|
||||
* 其中每一个自由度都包括一个串级 PID 控制器，Rate 控制和 Attitude 控制，前者以角速度作为输入量，控制角度修正的速度，后者以拟合后的角度为输入量，控制飞机到达目标角度，两个控制器以不同的频率配合工作。当然，也可以选择只使用单级的 PID 控制，默认情况下 pitch 和 roll 自由度使用 Attitude 控制，yaw 使用 Rate 控制。
|
||||
|
||||
```
|
||||
可以在crtp_commander_rpyt.c中调整如下参数选择
|
||||
static RPYType stabilizationModeRoll = ANGLE; // Current stabilization type of roll (rate or angle)
|
||||
static RPYType stabilizationModePitch = ANGLE; // Current stabilization type of pitch (rate or angle)
|
||||
static RPYType stabilizationModeYaw = RATE; // Current stabilization type of yaw (rate or angle)
|
||||
```
|
||||
|
||||
**实现代码**
|
||||
|
||||
```
|
||||
void controllerPid(control_t *control, setpoint_t *setpoint,
|
||||
const sensorData_t *sensors,
|
||||
const state_t *state,
|
||||
const uint32_t tick)
|
||||
{
|
||||
if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) { //该宏定义用于控制PID的计算频率，时间基准来自MPU6050触发的中断
|
||||
// Rate-controled YAW is moving YAW angle setpoint
|
||||
if (setpoint->mode.yaw == modeVelocity) { //rata模式,对yaw做修正
|
||||
attitudeDesired.yaw += setpoint->attitudeRate.yaw * ATTITUDE_UPDATE_DT;
|
||||
while (attitudeDesired.yaw > 180.0f)
|
||||
attitudeDesired.yaw -= 360.0f;
|
||||
while (attitudeDesired.yaw < -180.0f)
|
||||
attitudeDesired.yaw += 360.0f;
|
||||
} else { //attitude模式
|
||||
attitudeDesired.yaw = setpoint->attitude.yaw;
|
||||
}
|
||||
}
|
||||
|
||||
if (RATE_DO_EXECUTE(POSITION_RATE, tick)) { //位置控制
|
||||
positionController(&actuatorThrust, &attitudeDesired, setpoint, state);
|
||||
}
|
||||
|
||||
if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
|
||||
// Switch between manual and automatic position control
|
||||
if (setpoint->mode.z == modeDisable) {
|
||||
actuatorThrust = setpoint->thrust;
|
||||
}
|
||||
if (setpoint->mode.x == modeDisable || setpoint->mode.y == modeDisable) {
|
||||
attitudeDesired.roll = setpoint->attitude.roll;
|
||||
attitudeDesired.pitch = setpoint->attitude.pitch;
|
||||
}
|
||||
|
||||
attitudeControllerCorrectAttitudePID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw,
|
||||
attitudeDesired.roll, attitudeDesired.pitch, attitudeDesired.yaw,
|
||||
&rateDesired.roll, &rateDesired.pitch, &rateDesired.yaw);
|
||||
|
||||
// For roll and pitch, if velocity mode, overwrite rateDesired with the setpoint
|
||||
// value. Also reset the PID to avoid error buildup, which can lead to unstable
|
||||
// behavior if level mode is engaged later
|
||||
if (setpoint->mode.roll == modeVelocity) {
|
||||
rateDesired.roll = setpoint->attitudeRate.roll;
|
||||
attitudeControllerResetRollAttitudePID();
|
||||
}
|
||||
if (setpoint->mode.pitch == modeVelocity) {
|
||||
rateDesired.pitch = setpoint->attitudeRate.pitch;
|
||||
attitudeControllerResetPitchAttitudePID();
|
||||
}
|
||||
|
||||
// TODO: Investigate possibility to subtract gyro drift.
|
||||
attitudeControllerCorrectRatePID(sensors->gyro.x, -sensors->gyro.y, sensors->gyro.z,
|
||||
rateDesired.roll, rateDesired.pitch, rateDesired.yaw);
|
||||
|
||||
attitudeControllerGetActuatorOutput(&control->roll,
|
||||
&control->pitch,
|
||||
&control->yaw);
|
||||
|
||||
control->yaw = -control->yaw;
|
||||
}
|
||||
|
||||
if (tiltCompensationEnabled)
|
||||
{
|
||||
control->thrust = actuatorThrust / sensfusion6GetInvThrustCompensationForTilt();
|
||||
}
|
||||
else
|
||||
{
|
||||
control->thrust = actuatorThrust;
|
||||
}
|
||||
|
||||
if (control->thrust == 0)
|
||||
{
|
||||
control->thrust = 0;
|
||||
control->roll = 0;
|
||||
control->pitch = 0;
|
||||
control->yaw = 0;
|
||||
|
||||
attitudeControllerResetAllPID();
|
||||
positionControllerResetAllPID();
|
||||
|
||||
// Reset the calculated YAW angle for rate control
|
||||
attitudeDesired.yaw = state->attitude.yaw;
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
### Mellinger 控制器
|
||||
|
||||
Mellinger 控制器是一种“多合一” 控制器，它基于目标位置和目标位置速度矢量，直接计算出需要分配给所有电动机的所需推力。
|
||||
|
||||
详情可参考论文：[Minimum snap trajectory generation and control for quadrotors](https://ieeexplore.ieee.org/abstract/document/5980409)
|
||||
|
||||
### INDI 控制器
|
||||
|
||||
INDI 控制器是立即处理角速率以确定信任度的控制器，与传统的 PID 控制器相结合，对于角度处理相比串级 PID 控制器组合的速度要快。
|
||||
|
||||
详情可参考论文：[Adaptive Incremental Nonlinear Dynamic Inversion for Attitude Control of Micro Air Vehicles](https://arc.aiaa.org/doi/pdf/10.2514/1.G001490)
|
||||
|
||||
|
||||
## PID 参数整定
|
||||
|
||||
**crazyflie `Rate PID`调整过程**
|
||||
|
||||
1. 先调整`Rate `模式，将`rollType `,`pitchType` 和 `yawType`都调整为`RATE`
|
||||
2. 将 `ATTITUDE`模式对应的 `roll`, `pitch` 和 `yaw`的`KP`,`KI`和`KD`调整为`0.0`，仅保留`Rate `相关的参数
|
||||
3. 将`RATE`模式对应的 `roll`, `pitch` 和 `yaw` 的`KI`和`KD`调整为`0.0`，先调整比例控制`KP`
|
||||
4. 烧写代码，使用cfclient的param功能开始在线进行`KP`的调整
|
||||
5. 注意，使用cfclient修改后的参数，掉电是不保存的。
|
||||
6. 注意安全，因为在PID调整期间会出现超调的情况
|
||||
7. 先固定住飞行器，让其只能进行`pitch`轴的翻转。逐渐增加`pitch`对应的`KP`,直到飞机出现前后的震荡（超调）
|
||||
8. 当出现严重的震荡时，可以稍微降低`KP`（ Once you reach the point of instability, tune it down 5-10 points），然后即可确定`KP`参数
|
||||
9. 同样的方法调整 `roll`
|
||||
10. 最后同样的方法调整`yaw`
|
||||
11. 下面调整 `KI`，该参数用于消除稳态误差，因为如果不引入该参数，只有比例调整的话，飞机受到重力等干扰会在0位置上线摆动。设置 `KI`的初始值为`KP`的50%。
|
||||
12. 当`KI`增大到一定程度，也会导致飞机不稳定的晃动，但是`KI`造成的晃动频率会相比`KP`带来的震动，频率更小。然后以造成这个状态的 `KI`为基础确定 `KI`的值（This is your critical KI, and so tune down 5-10 points.）
|
||||
13. 同样的方法调整 `roll` 和 `yaw`
|
||||
14. yaw axis, except KI is usually around 80%+ of KP.
|
||||
|
||||
****
|
||||
以上完成了`Rate `模式参数的调整
|
||||
****
|
||||
|
||||
**下面开始整定 `Attitude PID`**
|
||||
|
||||
14. 确保`Rate PID`调整已经完成。
|
||||
15. 将`rollType `,`pitchType` 和 `yawType`都调整为`ANGLE`，意味着飞机已经进入attitude mode。
|
||||
16. 改变 `roll`和`pitch`的`KI`和`KD`为`0.0`，将`Yaw` 的 `KP``KI``KD`都设置为`0.0` 。
|
||||
17. 烧写代码，使用cfclient的param功能开始在线进行`KP`的调整。
|
||||
18. 将`roll`和`pitch`的`KP `设置为`3.5`，寻找任何不稳定性，例如振荡。持续增加KP，直到达到极限。
|
||||
19. 如果您发现`KP`导致不稳定，如果此时已经高于`4`，需要将`RATE`模式的 `KP`和`KI`稍微降低5-10点。这使您在调整姿势模式时更加“自由”
|
||||
20. 要调整KI，请再次缓慢增加KI。不稳定性的状态是产生低频振荡。
|
||||
@@ -1,224 +0,0 @@
|
||||
|
||||
## VL53LXX 传感器特性
|
||||
|
||||
**工作原理**
|
||||
|
||||
VL53L0X / VL53L1X 芯片内部集成了激光发射器和 SPAD 红外接收器，通过探测光子发送和接收时间差，计算光子飞行距离，最远测量距离可达两米，适合中短距离测量的应用。
|
||||
|
||||
|
||||
|
||||
**测量区域 - ROI**
|
||||
|
||||
VL53L0X / VL53L1X 的测量值为测量区域中的最短距离，测量区域可以根据使用场景进行放大或收缩，较大的探测范围可能会引起测量值的波动。
|
||||
|
||||
> 测量区域的配置参见 2.4 ranging description 2.8 Sensing array optical center
|
||||
|
||||
|
||||
|
||||
**测量距离**
|
||||
|
||||
* VL53L0X 传感器存在 **3-4 cm 的盲区**，有效测量范围 3cm-200cm，精度 +-3%
|
||||
* VL53L1X 是 VL53L0X 的升级版本，探测距离可达 400 cm
|
||||
|
||||
|
||||
|
||||
* VL53LXX 测量距离与光线环境有关，黑暗环境下可获得更高的探测距离，在室外强光下，激光传感器可能会受到很大的干扰，导致测量精度降低，因此室外需要结合气压定高。
|
||||
|
||||
|
||||
|
||||
**测量频率**
|
||||
|
||||
* VL53L0X 响应频率最快可达 50 Hz，测量误差+-5%
|
||||
* VL53L1X I2C 最大时钟频率可达 400khz，上拉电阻需要根据电压和总线电容值选择，可以参见vl53l1x datasheet
|
||||
|
||||
|
||||
|
||||
* XSHUT 为输入引脚，用于模式选择（休眠），需要上拉电阻放置漏电流
|
||||
* GPIO1 为中断输出引脚，用于输出测量 dataready 中断
|
||||
|
||||
**工作模式**
|
||||
|
||||
通过设置 XSHUT 引脚的电平，可以切换传感器进入 HW Standby 模式或 SW Standby 模式，实现有条件的启动，降低待机功耗。如果主机放弃对传感器模式进行管理，可将 XSHUT 引脚默认为上拉。
|
||||
|
||||
* HW Standby ：XSHUT拉低时，传感器电源被关闭
|
||||
* SW Standby ：XSHUT拉高，进入boot和SW Standby 模式
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
## VL53LXX 初始化步骤
|
||||
|
||||
1. 等待硬件初始化完成
|
||||
2. 数据初始化
|
||||
3. 静态初始化，装载数据
|
||||
4. 设置测量距离模式
|
||||
5. 设置单次测量最长等待时间
|
||||
6. 设置测量频率（时间间隔）
|
||||
7. 设置测量区域 ROI（可选）
|
||||
8. 启动测量
|
||||
|
||||
```text
|
||||
/*init vl53l1 module*/
|
||||
void vl53l1_init()
|
||||
{
|
||||
|
||||
Roi0.TopLeftX = 0; //测量目标区 可选最小4*4，最大16*16
|
||||
Roi0.TopLeftY = 15;
|
||||
Roi0.BotRightX = 7;
|
||||
Roi0.BotRightY = 0;
|
||||
Roi1.TopLeftX = 8;
|
||||
Roi1.TopLeftY = 15;
|
||||
Roi1.BotRightX = 15;
|
||||
Roi1.BotRightY = 0;
|
||||
|
||||
int status = VL53L1_WaitDeviceBooted(Dev); //等待硬件初始化完成
|
||||
status = VL53L1_DataInit(Dev); //数据初始化，上电后立刻执行
|
||||
status = VL53L1_StaticInit(Dev); //静态初始化，装载参数
|
||||
status = VL53L1_SetDistanceMode(Dev, VL53L1_DISTANCEMODE_LONG);//设置测量模式
|
||||
status = VL53L1_SetMeasurementTimingBudgetMicroSeconds(Dev, 50000); //设置最长时间，根据测量模式确定
|
||||
status = VL53L1_SetInterMeasurementPeriodMilliSeconds(Dev, 100); //测量间隔
|
||||
|
||||
status = VL53L1_SetUserROI(Dev, &Roi0); //设置ROI
|
||||
status = VL53L1_StartMeasurement(Dev); //启动测量
|
||||
if(status) {
|
||||
printf("VL53L1_StartMeasurement failed \n");
|
||||
while(1);
|
||||
}
|
||||
|
||||
}
|
||||
```
|
||||
|
||||
* 以上初始化步骤除 VL53L1\_SetUserROI，其余不可少
|
||||
|
||||
## VL53LXX 测距步骤
|
||||
|
||||
**轮寻测量模式**
|
||||
|
||||
轮训测量流程图：
|
||||
|
||||
|
||||
|
||||
* 注意在完成一次测量和读取后，需要使用`VL53L1_ClearInterruptAndStartMeasurement`清除中断标志并重新开始。
|
||||
* 轮训测量有两种方法，如上图所示，一种是阻塞方式（drivers polling mode），一种是非阻塞方式（Host polling mode），以下代码为阻塞测量方式。
|
||||
|
||||
```text
|
||||
/* Autonomous ranging loop*/
|
||||
static void
|
||||
AutonomousLowPowerRangingTest(void)
|
||||
{
|
||||
printf("Autonomous Ranging Test\n");
|
||||
|
||||
static VL53L1_RangingMeasurementData_t RangingData;
|
||||
VL53L1_UserRoi_t Roi1;
|
||||
int roi = 0;
|
||||
float left = 0, right = 0;
|
||||
if (0/*isInterrupt*/) {
|
||||
} else {
|
||||
do // polling mode
|
||||
{
|
||||
int status = VL53L1_WaitMeasurementDataReady(Dev); //等待测量结果
|
||||
if(!status) {
|
||||
status = VL53L1_GetRangingMeasurementData(Dev, &RangingData); //获取单次测量数据
|
||||
if(status==0) {
|
||||
if (roi & 1) {
|
||||
left = RangingData.RangeMilliMeter;
|
||||
printf("L %3.1f R %3.1f\n", right/10.0, left/10.0);
|
||||
} else
|
||||
right = RangingData.RangeMilliMeter;
|
||||
}
|
||||
if (++roi & 1) {
|
||||
status = VL53L1_SetUserROI(Dev, &Roi1);
|
||||
} else {
|
||||
status = VL53L1_SetUserROI(Dev, &Roi0);
|
||||
}
|
||||
status = VL53L1_ClearInterruptAndStartMeasurement(Dev); //释放中断
|
||||
}
|
||||
}
|
||||
while (1);
|
||||
}
|
||||
// return status;
|
||||
}
|
||||
```
|
||||
|
||||
**中断测量模式**
|
||||
|
||||
中断测量模式需要使用中断引脚 GPIO1，在数据 ready 时，GPIO1 引脚电平将被拉低，通知主机进行数据读取。
|
||||
|
||||
|
||||
|
||||
## 传感器校准
|
||||
|
||||
如果在传感器接收器上方添加了光罩，或者传感器藏在透明的盖板背后，由于透光率的变化，需要对传感器进行校准，可以根据校准流程调用 API 编写校准程序，也可以使用官方提供的 GUI 上位机直接测量出校准值。
|
||||
|
||||
**使用官方 API 编写校准程序**
|
||||
|
||||
校准流程：调用顺序要完全一致。
|
||||
|
||||
|
||||
|
||||
```
|
||||
/*Calibration vl53l1 module*/
|
||||
static VL53L1_CalibrationData_t vl53l1_calibration(VL53L1_Dev_t *dev)
|
||||
{
|
||||
int status;
|
||||
int32_t targetDistanceMilliMeter = 703;
|
||||
VL53L1_CalibrationData_t calibrationData;
|
||||
status = VL53L1_WaitDeviceBooted(dev);
|
||||
status = VL53L1_DataInit(dev); //performs the device initialization
|
||||
status = VL53L1_StaticInit(dev); // load device settings specific for a given use case.
|
||||
status = VL53L1_SetPresetMode(dev,VL53L1_PRESETMODE_AUTONOMOUS);
|
||||
status = VL53L1_PerformRefSpadManagement(dev);
|
||||
status = VL53L1_PerformOffsetCalibration(dev,targetDistanceMilliMeter);
|
||||
status = VL53L1_PerformSingleTargetXTalkCalibration(dev,targetDistanceMilliMeter);
|
||||
status = VL53L1_GetCalibrationData(dev,&calibrationData);
|
||||
|
||||
if (status)
|
||||
{
|
||||
ESP_LOGE(TAG, "vl53l1_calibration failed \n");
|
||||
calibrationData.struct_version = 0;
|
||||
return calibrationData;
|
||||
|
||||
}else
|
||||
{
|
||||
ESP_LOGI(TAG, "vl53l1_calibration done ! version = %u \n",calibrationData.struct_version);
|
||||
return calibrationData;
|
||||
}
|
||||
|
||||
}
|
||||
```
|
||||
|
||||
**使用官方 GUI 上位机校准传感器**
|
||||
|
||||
官方提供了用于配置和校准传感器的 GUI 上位机，配合 ST 官方 `STM32F401RE nucleo` 开发板连接传感器，使用软件校准得到基准值后，初始化时填入的即可。
|
||||
|
||||
>
|
||||
|
||||
>[STSW-IMG008:Windows Graphical User Interface \(GUI\) for VL53L1X Nucleo packs. Works with P-NUCLEO-53L1A1 ](https://www.st.com/content/st_com/en/products/embedded-software/proximity-sensors-software/stsw-img008.html)
|
||||
|
||||
## ESP32 + VL53L1x 例程
|
||||
|
||||
**例程说明**
|
||||
|
||||
1. 实现功能：通过VL53L1x 检测到高度变化（持续一秒），红灯亮起。高度恢复正常值（持续一秒），绿灯亮起。
|
||||
2. 可配置参数：通过make menuconfig 设置I2C 号码、端口号、LED端口号
|
||||
3. 例程解析见代码注释与用户手册
|
||||
|
||||
**注意事项**
|
||||
|
||||
4. 该例程只适用于VL53L1x，寄送的传感器为该型号。VL53L0x为老版本硬件，不适用本例程。
|
||||
5. 官方标称400cm测量距离，为黑暗环境下测得。室内正常灯光环境，可以保证10cm-260cm范围的有效测量
|
||||
6. 初始化函数vl53l1\_init（VL53L1\_Dev\_t \*） 中部分参数，需要根据实际使用环境确定，还有优化的空间。
|
||||
7. 传感器安装位置应确保在检测位置正上方
|
||||
8. 模块上电时自动矫正基准高度，如果基准高度有变化，需要重新上电重置参数
|
||||
|
||||
**例程仓库**
|
||||
|
||||
[esp32-vl53l1x-test](https://github.com/qljz1993/esp32-vl53l1x-test/tree/master) 或者：
|
||||
|
||||
```text
|
||||
git clone https://github.com/qljz1993/esp32-vl53l1x-test.git
|
||||
```
|
||||
|
||||
Reference in New Issue
Block a user