实例教程：BBDB为AHRS算法开发提供完善的支撑环境（上）

news2024/11/19 8:33:42

1. 概述

本教程将结合程序代码及CSS控制站工程，讲述如何基于PH47代码框架的BBDB固件版本，为开发自己的AHRS姿态解算算法提供完善支撑环境，以及数据分析手段。

BBDB固件已内置了一套姿态解算算法。对于需要进行AHRS算法开发研究的开发者，可在BBDB已有AHRS算法基础上另行增加自己的算法进行性能比对，或使用自己的算法模块替换预置AHRS解算模块。

本节教程就在PH47代码框架的应用层实现AHRS算法开发环境，以及如何使用CSS控制站工程显示、分析、记录相关算法数据进行讲述。关于在算法层实现对AHRS算法的支撑，以及如何使用自己的AHRS算法替换BBDB预置的算法模块，将在下节教程讲述。

2. BBDB基础上为算法实现提供支撑方式一：应用层实现

2.1. 代码初始化与解算代码的实现位置

通常我们的算法都以模块化方式实现，既方便程序设计，又利于将来代码复用。算法模块能够以一个或多个C++函数的方式进行组织，也能够通过封装为C++类的方式进行实现。

基于PH47代码框架BBDB固件版本，用户的AHRS算法模块既可以简便的在的应用层（Application）实现；又可以进行更好的封装后在算法层（Algorithms）中实现。本节就首先对在应用层的实现方式进行讲解。

此方式下，用户的AHRS代码可在BBDB的应用层实现类CAppBBDB（\DevStudio\Application\App_BBDB.cpp/.h）中实现或调用。具体如下：

函数名称

AHRS代码实现

CAppBBDB.

Init()

PH47框架初始化函数。

用于AHRS算法初始化功能实现。本函数在PH47代码框架初始化过程调用，可安全使用PH47的各项调试或其他功能函数。

注意：

在本函数调用之前快速线程函数如FastThread_1000Hz(),

NormalThread_250Hz() 已经开始运行，所以位于上述函数中的代码应对初始化完成情况进行判断（借助标志变量core.blSysInitCompleted或全局状态变量S_INIT_COMPLETE）

CAppBBDB.

FastThread_1000Hz()

PH47框架快速线程函数。

可用于AHRS解算代码的具体实现。根据控制板mcu不同型号，调用频率在400-500hz间波动。可通过相关函数获取两次调用间的时间间隔（后面详述）。

注意：

AHRS解算时间必须小于函数调用周期（2-2.5ms），事实上，由于在该线程中还有框架自身的函数运行，故实际解算时间要更短。

关于本线程全部的运行耗时，可通过在调试串口输入”freemem;”命令进行查询。

CAppBBDB.

NormalThread_250Hz()

PH47框架普通线程函数。

可用于AHRS解算代码的具体实现。调用频率固定为250hz。

关于对AHRS运行时间的要求与FastThread_1000Hz()函数相同。

2.2. AHRS相关的输入数据

AHRS算法部分以模块化方式实现，那么模块的输入输出数据交换就是必要的一个环节。PH47代码框架以数据总线、全局状态变量、机载控制参数3种不同的形式将相关的数据提供给各功能模块使用。

除了框架提供的数据之外，用户还可以根据自己的需要，向数据总线、全局状态变量、机载控制参数中加入自己定制的数据。

与AHRS解算模块的相关输入数据如下（如无特殊说明，数据单位均为国际标准单位，以下均同）：

IMU相关数据

数据类型

数据说明

IMU.GyrRaw

Vector3f

陀螺原始角速度（x,y.z三轴，下同）

IMU.GyrFilted

Vector3f

陀螺滤波后角速度

IMU.AccRaw

Vector3f

加速度计原始加速度

IMU.AccFilted

Vector3f

加速度计滤波后加速度

IMU.MagRaw

Vector3f

磁强计原始磁强（无单位）

IMU.MagAvg

Vector3f

磁强计平均磁强（无单位）

IMU.Temp

IMU_TEMPERATURE

IMU温度数据（Gyro温度x3，A储存温度x1）

GPS相关数据

数据类型

数据说明

sGps.fix_type

uint8_t_

gps锁定类型（无单位）

sGps.sat_num

uint8_t_

卫星数量（无单位）

sGps.loc  

Location

当前定位数据，

Location.lng，Location.lat单位为1e-7degree，

Location.alt单位为mm

sGps.vel

Vector3f

当前gps 3维速度矢量

sGps.eph

float

垂直定位精度

sGps.epv

float

水平定位精度

sGps.groundspeed

float

地速

sGps.speed_acc

float

速度精度

sGps.p_dop

float

位置定位精度

sGps.cog

float

gps航迹角

sGps.time_ms

uint32_t

sGps.

locked_time_ms

uint32_t

sGps.locked_dt

float

全局状态变量

数据说明

S_IMU_ENABLE

IMU初始化工作完成标志，true表示初始化成功

S_MAG_ENBALE

磁强计初始化标志，true表示初始化完成

S_COMPASS_BE_USED

磁强计数据有效标志

true表示根据磁强数据计算的航向角误差在允许范围内

2.3. AHRS解算后输出数据

经AHRS解算后的数据同样可以返回到数据总线、全局状态变量、机载控制参数当中。框架中与AHRS输出相关的预置数据如下：

AHRS输出数据

数据类型

数据说明

AngleRate

Vector3f

机体平台三轴转动角速率

AccelBody

Vector3f

机体坐标系三轴加速度

AccelNed

Vector3f

North-East-Down坐标系三轴加速度

sAhrs.Roll

float

机体滚转角

sAhrs.Pitch

float

机体俯仰角

sAhrs.Yaw

float

机体航向角

sAhrs.SinRoll

float

滚转角正弦值

sAhrs.SinPitch

float

俯仰角正弦值

sAhrs.SinYaw

float

航向角正弦值

sAhrs.CosRoll

float

滚转角余弦值

sAhrs.CosPitch

float

俯仰角余弦值

sAhrs.CosYaw

float

航向角余弦值

sAhrs.dcm

float

方向余弦矩阵

2.4. 解算频率，及对数据时间间隔的使用

AHRS解算循环当中的时间间隔数据对算法实现非常重要，如前文所述，CAppBBDB.FastThread_1000Hz()函数运行频率是一个在1000-1500hz之间波动的一个数值（波动范围根据控制板MCU不同而不同），但实际上，在基于PH47框架的程序开发中：

强烈不推荐使用循环函数的设计运行频率来计算循环时间间隔。

建议使用实际测量的循环时间间隔，或者使用总线数据设置时间间隔来作为算法设计所需要的时间间隔。框架提供了一个简单的类CLoopDt用于循环时间间隔的测量，示例代码如下：

// Step1：在.h文件中定义CloopDt类对象 _QuatDt
CloopDt _QuatDt;        
// Step2 在.cpp文件的循环代码中获取当前时刻距离上一次调用本函数时刻的
// 时间差，以ms为单位，精确到小数点后三位
// 如果过获取的dt为0，则返回 false
if( _QuatDt.GetLocalDt_ms(_fDt_Sec) )
   _fDt_Sec /= 1000.0f;               // 将获取时间间隔从ms转换为sec

通过上述代码即可精确的获取循环运行时间间隔。通过此方式获取的时间间隔，一般称为本地时间间隔。与之相对应的是原始时间间隔。

原始时间间隔是指通过总线数据的时间戳计算获取的时间间隔，因为该时间戳是总线数据被设置时刻记录的时间（精确到us），故总线数据两次被设置之间的间隔就称为原始时间间隔。通过简单函数调用就可获得总线数据的原始时间间隔：

// 获取陀螺仪原始角速度的设置时间间隔
float fRawGyrDt_ms = bus.sImu.GyrRaw.GetDt2Prev_us()/1000.0f;  

在AHRS算法实现过程中，应当仔细和小心的确定在什么时候使用本地时间间隔，在什么时候该使用原始时间间隔。

2.5. 应用层代码框架示例

若我们在应用层进行AHRS算法模块的开发，那么该模块的初始化函数为InitMyAHRS()，在CAppBBDB.Init() 函数中被调用。

姿态解算函数为UpdateMyAHRS()，在CAppBBDB.FastThread_1000Hz()，或是在CAppBBDB.NormalThread_250Hz()函数中被调用。

在App_BBDB.h文件中对函数进行声明。当然，AHRS解算必须的一些成员变量也可以在.h文件中进行声明。

// AHRS 应用层框架代码
void InitMyAHRS();
bool UpdateMyAHRS();

// AHRS解算所需的成员变量声明…
// ...

在App_BBDB.cpp文件中实现函数InitMyAHRS()及UpdateMyAHRS()：

通过上述代码框架框架实现，用户即可在上述框架中实现自己的姿态解算算法，并将于姿态相关的各种数据，通过如下数据帧下行发送至CSS进行显示、记录、回放，以及后续数据分析使用：

数据帧

(mavlink message)

帧id

(msg id)

包含字段

(Field of mavlink message)

GPS_RAW_INT

0x18

lat, Lon, alt

eph, epv, vel, cog, fix_type, satellites_visi

RAW_IMU

0x1b

xacc, yacc, zacc,

xgyro, ygyro, zgyro,

xmag, ymag, zmag

ATTITUDE

0x1e

roll, pitch, yaw,

rollspeed, pitchspeed, yawspeed

PILOT_DBG_1

0xf7

fvalue_0 – fvalue_9

PILOT_DBG_2

0xf8

fvalue_0 – fvalue_9

备注：

  1. 以上数据帧仅为BBDB固件默认的下行数据，若需要增加新的下行数据，可使用通用数据帧USER_DEF_DATA(0x7a)或COMMAND_LONG(0x4c)进行下行传输
  2. 上述数据帧当中所包含的字段定义，可参见CSS帧编辑器。
void CAppBBDB::InitMyAHRS()
{
	/******************************************************
	AHRS 算法初始化函数
	本函数在系统初始化过程中调用
	函数调用顺序: 
	CThreadCtrl_BBDB.InitAfterThreadStart() ==> CAppBBDB.Init()
	==> CAppBBDB.InitMyAHRS()
	******************************************************/

	/******************************************************
	*	AHRS 初始化过程实现 ...
	******************************************************/
}

bool CAppBBDB::UpdateMyAHRS()
{
	/******************************************************
	AHRS 算法实现函数
	本函数在 CAppBBDB.FastThread_1000Hz() 或 
	CAppBBDB.NormalThread_250Hz() 函数中被调用
	******************************************************/

	// 若系统初始化(即包含了 InitMyAHRS() 初始化)还未完成，则暂不进行 AHRS 解算过程
	if(core.blSysInitCompleted == false)
		return false;
	
	// 获取陀螺仪原始角速度的设置时间间隔
	// imu 当中的 AccRaw, GyrRaw 数据在 CThreadCtrl_BBDB.IdleLoop() 函数中被近乎同时的进行设置(< 1us)
	// 故可用 AccRaw, GyrRaw 当中的任一原始时间间隔作为这两个数据被设置的时间间隔 
	float fDt_ms_RawGyr = bus.sImu.GyrRaw.GetDt2Prev_us() / 1000.0f;

	Vector3f vGyrRaw = bus.sImu.GyrRaw.Get();	// 获取陀螺仪原始角速度
	Vector3f vAccRaw = bus.sImu.AccRaw.Get();	// 获取加速度计原始数据
	Vector3f vMagAvg = bus.sImu.MagAvg.Get();	// 获取磁强计测量的平均磁强
	float fCog = bus.sGps.cog.Get();			// 获取GPS计算的飞行航迹角
	// 其他 AHRS 解算所需数据 ...

	bool blImuInit = gGetStatus(S_IMU_ENABLE);	// 获取 imu 初始化完成标志
	// 其他全局状态变量获取 ...

	float fGyrLPF = core.para.Get(P_SENSOR_GYR_LPF_Hz);		// 获取陀螺仪低通滤波器截止频率
	// 其他机载控制参数获取 ...

	/******************************************************
	* ...
	* AHRS 解算过程实现 ...
	* ...
	******************************************************/

	/******************************************************
	* AHRS 输出数据设置 ...
	bus.sAhrs.dcm.Set(_dcm);
	bus.sAhrs.Roll.Set(_euler.x);
	...
	******************************************************/

	/******************************************************
	* 如有必要, 对调试数据赋值 ...
	bus.arDbg_1[0].Set(_fDbg_x);
	bus.arDbg_1[6].Set(_fDbg_y);
	bus.arDbg_2[1].Set(_fDbg_z);
	******************************************************/

	/******************************************************
	* 如有必要, 根据 AHRS 解算结果设置全局变量 ...
	gSetStatus(S_MY_STATUS);
	******************************************************/
	return true;
}

3. 使用CSS的BBDB控制站工程提进行观测分析

在BBDB固件的支持下，AHRS算法设计中的相关数据通过前述预设数据帧下行发送到地面控制站（CSS），并通过对应的控制站工程CssDemo_BBDB进行解析、显示、记录，以及后续的分析。关于CSS的使用方法，可参见《Control Station Studio控制站开发平台概述》。以下仅简单讲述：

3.1. 接收并记录BBP控制板下行数据

选择“自动记录”模式后点击“GCS启动”启动CSS，开始接收、解析、显示、记录来自于BBP控制板的下行数据。

3.2. 使用仪表控件对AHRS数据进行显示

控制站工程CssDemo_BBDB能够以数字、姿态显示仪表、波形图等多种方式显示与AHRS相关的各种数据。若现有控件不足以满足数据观测需求，可对已有控件的显示属性进行修改，或增加新的控件（如增加新的波形图控件叠加显示多路数据）。

3.3. 对AHRS数据进行初步分析

将CSS切换到“数据分析”页面，打开先前的数据记录文件，选择一个或多个需要进行分析的数据（数据帧中字段）绘制数据曲线供分析使用。在分析过程中，可通过设定曲线的原点、缩放比例等方式进行辅助。

3.4. 导出记录数供进一步分析

切换“数据分析”页面，点击“导出txt”按钮，打开对应的数据记录文件，选择需要包含导出数据的数据帧，选择确定后即可将控制板记录数据导出为txt格式供进一步分析使用。

更多内容见CSDN博客专栏：无人机飞控icon-default.png?t=O83Ahttps://blog.csdn.net/ss15/category_9690939.html?spm=1001.2014.3001.5482相关资源：PH47: PH47运动控制代码框架.icon-default.png?t=O83Ahttps://gitee.com/ss15/ph47

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/2243281.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

Linux操作系统 ----- (5.系统管理)

Linux操作系统 ----- (5.系统管理)

目录 1.总结 2.本章学习目标 3.图形界面管理 3.1.X-Window图形界面概述 3.2.X-Window的结构 3.3.X-Window的特点 3.4.UKUI图形环境 3.5.桌面 3.5.1.桌面图标 3.5.2.计算机属性 3.5.3.桌面快捷菜单 3.6.任务栏 3.6.1.开始菜单 3.6.2.显示任务视图 3.6.3.文件管理器…
阅读更多...
hive复杂数据类型Array Map Struct 炸裂函数explode

hive复杂数据类型Array Map Struct 炸裂函数explode

1、Array的使用 create table tableName( ...... colName array<基本类型> ...... ) 说明&#xff1a;下标从0开始&#xff0c;越界不报错&#xff0c;以null代替 arr1.txtzhangsan 78,89,92,96 lisi 67,75,83,94 王五 23,12 新建表&#xff1a; create table arr1(n…
阅读更多...
基于Python实现的HDR图像处理算法

基于Python实现的HDR图像处理算法

此代码会读取两张图片&#xff0c;一张用于保留高光细节&#xff0c;另一张用于保留暗部细节。两张图片按指定比例进行像素融合&#xff0c;最终生成一张合成的HDR图片。 import cv2 import numpy as npdef hdr_fusion(highlight_img_path, shadow_img_path, output_path, alp…
阅读更多...
网络协议(4)拥塞控制

网络协议(4)拥塞控制

之前已经说过了tcp也是会考虑网络的情况的&#xff0c;也就是当网络出现问题的时候tcp不会再对报文进行重传。当所有的用户在网络不好的时候都不会对丢失的报文进行重传。这样就会防止网络瘫痪。 这样的机制也就是tcp会进行拥塞控制。 拥塞控制 所谓的慢启动看下面这张图就能…
阅读更多...
解决 IDEA 修改代码重启不生效的问题

解决 IDEA 修改代码重启不生效的问题

前言 在使用 IntelliJ IDEA 进行 Java 项目开发时&#xff0c;有时会遇到一个令人头疼的问题&#xff1a;修改了代码后&#xff0c;重启服务却发现更改没有生效。通常情况下&#xff0c;解决这个问题需要通过 Maven 的 clean 和 compile 命令来强制重新编译&#xff0c;但这显…
阅读更多...
git使用及上线流程（仅为我工作中常用）

git使用及上线流程（仅为我工作中常用）

推荐软件或者直接终端 ⚠️注意&#xff1a;在确保远程和本地分支都可使用的情况下 git常见使用命令 ls---查看所有目录 pwd---本机密码 cd 目录名---进入目录 Touch ---创建文本文件 git status---查看状态 git branch---查看分支 git pull---拉取远程最新代码 git checkou…
阅读更多...
12.C++内存管理1(C/C++内存分布，C语言动态内存管理)

12.C++内存管理1(C/C++内存分布，C语言动态内存管理)

⭐本篇重点&#xff1a;C/C内存分布&#xff0c;C语言动态内存管理 ⭐本篇代码&#xff1a;c学习/04.c-动态内存管理 橘子真甜/c-learning-of-yzc - 码云 - 开源中国 (gitee.com) 目录 一. C/C内存分布&#xff08;C/C内存地址空间&#xff09; 二. C语言动态内存管理 2.1 …
阅读更多...
游戏引擎学习第15天

游戏引擎学习第15天

视频参考:https://www.bilibili.com/video/BV1mbUBY7E24 关于游戏中文件输入输出&#xff08;IO&#xff09;操作的讨论。主要分为两类&#xff1a; 只读资产的加载 这部分主要涉及游戏中用于展示和运行的只读资源&#xff0c;例如音乐、音效、美术资源&#xff08;如 3D 模型和…
阅读更多...
JavaWeb——JS、Vue

JavaWeb——JS、Vue

目录 1.JavaScript a.概述 b.引入方式 c.JS的基础语法 d.JS函数 e.JS对象 f.JS事件监听 2.Vue a.概述 b.Vue常用指令 d.生命周期 1.JavaScript a.概述 JavaScript是一门跨平台、面向对象的脚本语言。是用来控制网页行为的&#xff0c;它能使网页可交互。JavaScript和…
阅读更多...
HarmonyOs鸿蒙开发实战(16)=＞沉浸式效果第一种方案一窗口全屏布局方案

HarmonyOs鸿蒙开发实战(16)=＞沉浸式效果第一种方案一窗口全屏布局方案

1.沉浸式效果的目的 开发应用沉浸式效果主要指通过调整状态栏、应用界面和导航条的显示效果来减少状态栏导航条等系统界面的突兀感&#xff0c;从而使用户获得最佳的UI体验。 2.窗口全屏布局方案介绍 调整布局系统为全屏布局&#xff0c;界面元素延伸到状态栏和导航条区域实现沉…
阅读更多...
spi 回环

spi 回环

///tx 极性0 &#xff08;sclk信号线空闲时为低电平&#xff09; /// 相位0 (在sclk信号线第一个跳变沿进行采样) timescale 1ns / 1ps//两个从机 8d01 8d02 module top(input clk ,input rst_n,input [7:0] addr ,input …
阅读更多...
CF862B Mahmoud and Ehab and the bipartiteness(二分图的性质）

CF862B Mahmoud and Ehab and the bipartiteness(二分图的性质）

思路&#xff1a;一个二分图是由两个集合组成的&#xff0c;同一个集合中的节点间不能连边&#xff0c;所以一个二分图最多有cnt[1]*cnt[2]条边&#xff0c;题目给出一个树的n-1条边&#xff0c;要我们添加最多的边数使他成为二分图&#xff0c;添加的边数就是cnt[1]*cnt[2]-n1…
阅读更多...
docker：基于Dockerfile镜像制作完整案例

docker：基于Dockerfile镜像制作完整案例

目录 摘要目录结构介绍起始目录package目录target目录sh目录init.sh脚本start.sh脚本stop.sh脚本restart.sh脚本 config目录 步骤1、编写dockerfilescript.sh脚本 2、构件镜像查看镜像 3、保存镜像到本地服务器4、复制镜像文件到指定目录&#xff0c;并执行init.sh脚本5、查看挂…
阅读更多...
Redis自学之路—基础数据结构具体方法解析（五）

Redis自学之路—基础数据结构具体方法解析（五）

目录 简介 数据结果具体方法解析 字符串（String） 操作命令 set设置值 setex setnx get获取值 del删除key mset批量设置值 incr数字运算 append追加指令 strlen字符串长度 getset设置并返回原值 setrange设置指定位置的字符 getrange截取字符串 命令的时间复杂…
阅读更多...
通过华为鲲鹏认证发行上市的集成平台产品推荐

通过华为鲲鹏认证发行上市的集成平台产品推荐

华为鲲鹏认证是技术实力与品质的权威象征&#xff0c;代表着产品达到了高标准的要求。从技术层面看&#xff0c;认证确保产品与华为鲲鹏架构深度融合&#xff0c;能充分释放鲲鹏芯片的高性能、低功耗优势&#xff0c;为集成平台的高效运行提供强大动力。在安全方面&#xff0c;…
阅读更多...
使用 AMD GPU 实现 Segment Anything

使用 AMD GPU 实现 Segment Anything

Segment Anything with AMD GPUs — ROCm Blogs 作者&#xff1a; Sean Song 发布日期&#xff1a;2024年6月4日 介绍 分割任务——识别图像中哪些像素属于某对象——是计算机视觉中的一个基础任务&#xff0c;应用广泛&#xff0c;从科学图像分析到照片编辑。Segment Anyth…
阅读更多...
Spring Cloud Stream实现数据流处理

Spring Cloud Stream实现数据流处理

1.什么是Spring Cloud Stream&#xff1f; 我看很多回答都是“为了屏蔽消息队列的差异&#xff0c;使我们在使用消息队列的时候能够用统一的一套API&#xff0c;无需关心具体的消息队列实现”。 这样理解是有些不全面的&#xff0c;Spring Cloud Stream的核心是Stream&#xf…
阅读更多...
无人机飞手入门指南

无人机飞手入门指南

无人机飞手入门指南旨在为初学者提供一份全面的学习路径和实践建议&#xff0c;帮助新手快速掌握无人机飞行技能并了解相关法规知识。以下是一份详细的入门指南&#xff1a; 一、了解无人机基础知识 1. 无人机构造&#xff1a;了解无人机的组成部分&#xff0c;如机身、螺旋桨…
阅读更多...
使用Mac下载MySQL修改密码

使用Mac下载MySQL修改密码

Mac下载MySQL MySQL官网链接MySQL​​​​​​ 当进入到官网后下滑到community社区&#xff0c;进行下载 然后选择community sever下载 这里就是要下载的界面&#xff0c;如果需要下载之前版本的话可以点击archives&#xff0c; 可能会因为这是外网原因&#xff0c;有时候下…
阅读更多...
两大新兴开发语言大比拼：Move PK Rust

两大新兴开发语言大比拼：Move PK Rust

了解 Move 和 Rust 的差异有助于开发者根据项目的具体需求选择最合适的语言。选择不恰当的语言可能会导致项目后期出现技术债务。不同语言有其独特的优势。了解 Move 和 Rust 的差异可以帮助开发者拓展技术视野&#xff0c;发现不同语言在不同领域的应用潜力。 咱们直奔主题&a…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023