陀螺仪LSM6DSV16X与AI集成(2)----姿态解算

陀螺仪LSM6DSV16X与AI集成.2--姿态解算

  • 概述
  • 视频教学
  • 样品申请
  • 完整代码下载
  • 欧拉角
  • 万向节死锁
  • 四元数法
  • 姿态解算
  • 双环PI控制器
  • 偏航角
  • 陀螺仪解析代码
  • 上位机通讯
  • 加速度演示
  • 陀螺仪工作方式
  • 主程序
  • 演示

概述

LSM6DSV16X包含三轴陀螺仪与三轴加速度计。
姿态有多种数学表示方式,常见的是四元数,欧拉角,矩阵和轴角。他们各自有其自身的优点,在不同的领域使用不同的表示方式。在四轴飞行器中使用到了四元数和欧拉角。
姿态解算选用的旋转顺序为ZYX,即IMU坐标系初始时刻与大地坐标系重合,然后依次绕自己的Z、Y、X轴进行旋转:
绕IMU的Z轴旋转:航向角yaw
绕IMU的Y轴旋转:俯仰角pitch
绕IMU的X轴旋转:横滚角row

最近在弄ST和瑞萨RA的课程,需要样片的可以加群申请:615061293 。

在这里插入图片描述

横滚roll,俯仰pitch,偏航yaw的实际含义如下图:

在这里插入图片描述
由于需要解析姿态角,故将陀螺仪速度修改快一点。

在这里插入图片描述

视频教学

https://www.bilibili.com/video/BV18M411d7Wg/

陀螺仪LSM6DSV16X与AI集成(2)----姿态解算

样品申请

https://www.wjx.top/vm/OhcKxJk.aspx#

完整代码下载

https://download.csdn.net/download/qq_24312945/88598263

欧拉角

横滚角φ:机体绕OBXB转动,轴Y’B与平面OBXBYB构成的夹角。
俯仰角θ:机体绕OBYB转动,轴Z’B与平面OBYBZB构成的夹角。
偏航角ψ:机体绕OBZB转动,轴X’B与平面OBXBZB构成的夹角。

在这里插入图片描述
将姿态角从机体坐标系转换到惯性坐标系中是为了便于分析无人机状态,反映无人机在惯性坐标系下的姿态运动状态,利用齐次线性变换可实现坐标系的转换,旋转矩阵就是在线性变化中产生的,用REB表示惯性坐标系{E}到机体坐标系{B}的变换。
例如,绕OBXB旋转必角,此时两个坐标系存在必的角度差,不再重合。点(x, y, z)的转换方程为:
在这里插入图片描述
可提取转换矩阵:
在这里插入图片描述

同理,绕口OBYB旋转θ角得:
在这里插入图片描述

而绕OBZB旋转ψ角得:
在这里插入图片描述

不同旋转顺序有不同的旋转矩阵,按照偏航,俯仰,横滚的顺序,即分别绕X-Y-Z旋转,就可计算出旋转矩阵REB,REB等于依次旋转所得的矩阵连乘,且顺序为从右向左排列。

在这里插入图片描述

万向节死锁

当俯仰角θ=±Π/2时,横滚运动与偏航运动的旋转轴重合,出现万向节死锁现象,在空间失去了一个自由度。如式所示,φ或ψ的变化具有相同的效果,因此不再具有唯一性啊。

在这里插入图片描述

四元数法

本文选择的是四元数法进行姿态解算。无人机姿态解算方法主要有四种,它们各自的优缺点如下图所示。欧拉角法不能用于计算飞行器的全姿态角,且难以实时计算而不易于工程应用。方向余弦法不会出现“奇点”现象,但计算量大,效率低。四元数法避免了复杂的三角函数运算,变为求解线性微分方程,算法简单易操作,且不存在角度奇异性问题,可以更好的线性化系统,是一种更实用的工程方法。

在这里插入图片描述

四元数的概念诞生在1843年的爱尔兰,是数学家哈密顿研究空间几何时提出。在如今的导航技术领域,四元数的优势逐渐被发现,得到了研究者们的广泛关注,并逐渐应用在姿态解算领域。

四元数是由四个元构成的数Q(q0,q1,q2,q3) = q0 + q1i + q2j + q3k;其中,q0,q1,q2,q3是实数,i,j,k既是互相正交的单位向量,又是虚单位根号-1。四元数即可看作四维空间中的一个向量,又可以看做一个超复数。对于后续有一个重要的变化需要记住:
Q=q0 + q1i + q2j + q3k
可视为一个超复数,Q 的共轭复数记为
Q’=q0 - q1i - q2j - q3k
Q°称为Q的共轭四元数。
同时,有
ij=k,jk=i,ki=j,ji=-k,kj=-i,ik=-j
i2 = j2 = k2 =ijk=-1
在这里插入图片描述
其中,i、j、k是相互正交的单位向量,其几何意义可理解为分别绕三个坐标轴的旋转,q0、q1、q2、q3为常数,有
在这里插入图片描述

通过四元数进行欧拉角求解,可以减少芯片运算负担,提高运算速度。
在这里插入图片描述

一个矢量V相对于坐标系OXYZ固定:V = xi + yj + zk;坐标系OXYZ转动了Q得到一个新坐标系OX’Y’Z’:V = x’i’ + y’j‘ + z’k’;设四元数Ve、Ve‘
Ve = xi + yj + zk;
Ve’ = x’i + y’j + z’k;
则Ve’ = Q* Ve * Q’;
设Q = q0 + q1i + q2j + q3k;则Q’ = q0 - q1i - q2j - q3k;
则Ve’ = Q* Ve * Q’=(q0 + q1i + q2j + q3k) * (0+xi + yj + zk) + (q0 - q1i - q2j - q3k)
可以算出
x’=(q0 ^2+q1 ^2-q2 ^2-q3 ^2)x+2(q1q2+ q1q3)y+2(q1q3-q0q2)z
y’ = 2(q1q2-q0q3)x+(q0 ^2-q1 ^2+q2 ^2-q3 ^2)y+2(q2q3+q0q1)z
z’ = 2(q1q3+q0q2)x+2(q2q3-q0q1)y+(q0 ^2-q1 ^2-q2 ^2+q3 ^2)z

在这里插入图片描述

结合
在这里插入图片描述
可以反推
在这里插入图片描述

        Pitch  = asin(2 * q2 * q3 + 2 * q0* q1)* 57.3; // pitch ,转换为度数Roll = atan2(-2 * q1 * q3 + 2 * q0 * q2, q0*q0-q1*q1-q2*q2+q3*q3)* 57.3; // rollvYaw = atan2(2*(q1*q2 - q0*q3),q0*q0-q1*q1+q2*q2-q3*q3) * 57.3;   //偏移太大,

将加速度的三维向量转为单位向量

        // 测量正常化norm = sqrt(ax*ax + ay*ay + az*az);      ax = ax / norm;                   //单位化ay = ay / norm;az = az / norm;    

世界坐标系重力分向量是通过方向旋转矩阵的最后一列的三个元素乘上加速度就可以算出机体坐标系中的重力向量。

        // 估计方向的重力vx = 2*(q1*q3 - q0*q2);//由下向上方向的加速度在加速度计X分量 vy = 2*(q0*q1 + q2*q3);//由下向上方向的加速度在加速度计X分量 vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;//由下向上方向的加速度在加速度计Z分量

姿态解算

在这里插入图片描述

双环PI控制器

陀螺仪能够迅速响应设备的旋转,在短时间内误差较小且可靠。然而,因为温度漂移、零漂移和积分误差会随时间累积,陀螺仪的长时间精度受到影响。在静止状态下,加速度计的漂移很小,其倾角求解过程中不存在积分误差,但在飞行过程中,加速度计受到发动机和机架振动以及转动和运动加速度的干扰。磁罗盘测量的地磁向量在特定地理范围内可视为不变,但磁罗盘易受硬磁场和软磁场干扰。
因此,若系统外环采用九轴姿态传感器(包括三轴加速度计、三轴磁罗盘和三轴陀螺仪)进行数据融合,磁罗盘易受干扰可能导致融合后的数据仍有较大误差。为此,在内环使用六轴姿态传感器(包括三轴加速度计和三轴陀螺仪)进行数据融合,对融合后的传感器姿态偏差进行二次修正,以提高整体精度。
外环九轴姿态传感器数据融合,记在飞行器机体坐标系下an=[ax ay az]T和mn=[mx my mz]T分别为加速度计和磁罗盘实际测量得到的重力向量和地磁向量。
记vn=[vx vy vz]T和wn=[mx my mz]T是将地理坐标系下重力向量kb=[0 0 1g]T和地磁向量nb=[nx 0 nz]T(不考虑地理磁偏角因素,将机头固定向北)通过四元数坐标换算成机体坐标系下的重力向量和地磁向量。向量之间的误差为坐标轴的旋转误差,可以用向量的叉积en=[ex ey ez]T表示,如下所示。
在这里插入图片描述

由于我的LSM6DS3TR-C为六轴,不带三轴陀螺仪,故代码如下。

//这个叉积向量仍旧是位于机体坐标系上的,而陀螺积分误差也是在机体坐标系,而且叉积的大小与陀螺积分误差成正比,正好拿来纠正陀螺。
//(你可以自己拿东西想象一下)由于陀螺是对机体直接积分,所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。ex = (ay*vz - az*vy);ey = (az*vx - ax*vz);ez = (ax*vy - ay*vx);

由于陀螺仪是对机体直接积分,所以,陀螺仪的误差可以体现为机体坐标的误差。因此修正坐标轴的误差可以达到修正陀螺仪误差的目的,从而将加速度计和磁罗盘进行修正陀螺仪,实现了九轴的数据融合。即如果陀螺仪按照叉积误差的轴,转动叉积误差的角度,就可以消除机体坐标上实际测量的重力向量和地磁向量和坐标换算后的重力向量和地磁向量之间的误差。
PI调节器的比例部分用于迅速纠正陀螺仪误差,积分部分用于消除稳态偏差。PI调节器的比例系数和积分系数自己去修正。陀螺仪经过外环PI控制器修正姿态误差后输出值为了gn =[gx gy gz]T
在这里插入图片描述

        // 积分误差比例积分增益,计算陀螺仪测量的重力向量与估计方向的重力向量之间的误差。exInt = exInt + ex*Ki;eyInt = eyInt + ey*Ki;ezInt = ezInt + ez*Ki;// 调整后的陀螺仪测量,使用叉积误差来进行比例-积分(PI)修正陀螺仪的零偏。将修正量乘以比例增益Kp,并加上之前计算的积分误差exInt、eyInt和ezInt。gx = gx + Kp*ex + exInt;gy = gy + Kp*ey + eyInt;gz = gz + Kp*ez + ezInt;
内环的六轴姿态传感器数据融合是将地理坐标系下的重力场向量与加速度计在机体坐标系下采集到的重力向量进行叉乘,求出两者向量误差。并通过PI控制器修正向量误差,从而达到修正外环九轴数据融合后的陀螺仪的偏差的目的。在每个姿态解算周期读取出机体坐标系下双环PI控制后的陀螺仪的角速率

在这里插入图片描述

整合四元数率和正常化,根据陀螺仪的测量值和比例-积分修正值,对四元数进行更新。
在这里插入图片描述

        // 整合四元数率和正常化,根据陀螺仪的测量值和比例-积分修正值,对四元数进行更新。根据微分方程的离散化形式,将四元数的每个分量加上相应的微分项乘以采样周期的一半(halfT)。q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  // 正常化四元数norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);q0 = q0 / norm;q1 = q1 / norm;q2 = q2 / norm;q3 = q3 / norm;

偏航角

六轴传感器(包括三轴加速度计和三轴陀螺仪)可以用于估算设备在空间中的姿态,包括俯仰角(Pitch)、横滚角(Roll)和偏航角(Yaw)。然而,六轴传感器仅依赖陀螺仪和加速度计数据,可能无法准确测量偏航角(Yaw),原因如下:
无磁场参考:六轴传感器缺少磁罗盘,没有固定的参考方向。因此,在长时间内,陀螺仪的积分误差可能导致偏航角估计漂移。
陀螺仪误差累积:陀螺仪测量的是角速度,要得到偏航角,需要将角速度积分。由于陀螺仪存在零漂、噪声和温度漂移等误差,这些误差在积分过程中会累积,使得偏航角估计产生较大的漂移。
虽然六轴传感器可能无法准确测量偏航角,但可以通过将其与磁罗盘(三轴磁场传感器)结合,形成九轴传感器(包括三轴加速度计、三轴磁罗盘和三轴陀螺仪),以提高偏航角估计的准确性。九轴传感器融合了磁场信息,为偏航角提供了一个稳定的参考方向,有助于减小陀螺仪误差对偏航角估计的影响。

陀螺仪解析代码

 //加速度单位g,陀螺仪rad/s
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{float norm;float vx, vy, vz;float ex, ey, ez;  // 测量正常化,把加计的三维向量转成单位向量。norm = sqrt(ax*ax + ay*ay + az*az);      ax = ax / norm;                   //单位化ay = ay / norm;az = az / norm;      // 估计方向的重力,世界坐标系重力分向量是通过方向旋转矩阵的最后一列的三个元素乘上加速度就可以算出机体坐标系中的重力向量。vx = 2*(q1*q3 - q0*q2);//由下向上方向的加速度在加速度计X分量 vy = 2*(q0*q1 + q2*q3);//由下向上方向的加速度在加速度计X分量 vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;//由下向上方向的加速度在加速度计Z分量//这个叉积向量仍旧是位于机体坐标系上的,而陀螺积分误差也是在机体坐标系,而且叉积的大小与陀螺积分误差成正比,正好拿来纠正陀螺。
//(你可以自己拿东西想象一下)由于陀螺是对机体直接积分,所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。ex = (ay*vz - az*vy);ey = (az*vx - ax*vz);ez = (ax*vy - ay*vx);// 积分误差比例积分增益,计算陀螺仪测量的重力向量与估计方向的重力向量之间的误差。exInt = exInt + ex*Ki;eyInt = eyInt + ey*Ki;ezInt = ezInt + ez*Ki;				// 调整后的陀螺仪测量,使用叉积误差来进行比例-积分(PI)修正陀螺仪的零偏。将修正量乘以比例增益Kp,并加上之前计算的积分误差exInt、eyInt和ezInt。gx = gx + Kp*ex + exInt;gy = gy + Kp*ey + eyInt;gz = gz + Kp*ez + ezInt; // 整合四元数率和正常化,根据陀螺仪的测量值和比例-积分修正值,对四元数进行更新。根据微分方程的离散化形式,将四元数的每个分量加上相应的微分项乘以采样周期的一半(halfT)。q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  // 正常化四元数norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);q0 = q0 / norm;q1 = q1 / norm;q2 = q2 / norm;q3 = q3 / norm;Pitch  = asin(2 * q2 * q3 + 2 * q0* q1)* 57.3; // pitch ,转换为度数Roll = atan2(-2 * q1 * q3 + 2 * q0 * q2, q0*q0-q1*q1-q2*q2+q3*q3)* 57.3; // rollvYaw = atan2(2*(q1*q2 - q0*q3),q0*q0-q1*q1+q2*q2-q3*q3) * 57.3;   //偏移太大,等我找一个好用的}

上位机通讯

这里使用的是匿名助手的上位机
https://gitee.com/anotc/AnoAssistant
有专门的通讯协议
在这里插入图片描述

串口通讯协议格式如下所示,需要注意传输为小端模式传输。

在这里插入图片描述
对应的源地址和目标地址分别为0xFD和0xFE。

在这里插入图片描述

我们只需要上报加速度和陀螺仪数据,所以功能码为0x01,数据长度为0x0D,需要主要为小端模式传输。

在这里插入图片描述

加速度演示

实测移动模块分别为X、Y、Z轴向下,可以看见数值基本上为1000mg。

在这里插入图片描述

陀螺仪工作方式

加速度计测量线性加速度,而陀螺仪测量角旋转。为此,他们测量了科里奥利效应产生的力。
陀螺仪是一种运动传感器,能够感测物体在一轴或多轴上的旋转角速度。它能够精确地感测自由空间中复杂的移动动作,因此成为追踪物体移动方位和旋转动作的必要设备。与加速计和电子罗盘不同,陀螺仪不需要依赖外部力量(如重力或磁场),可以自主地发挥其功能。因此,从理论上讲,只使用陀螺仪就可以完成姿态导航的任务。

在这里插入图片描述

陀螺仪的每个通道检测一个轴的旋转。也就是说陀螺仪通过测量自身的旋转状态,判断出设备当前运动状态,是向前、向后、向上、向下、向左还是向右呢,是加速(角速度)还是减速(角速度)呢,都可以实现,但是要判断出设备的方位(东西南北),陀螺仪就没有办法。
在这里插入图片描述

MEMS陀螺仪主要利用科里奥利力(旋转物体在有径向运动时所受到的切向力)原理,公开的微机械陀螺仪均采用振动物体传感角速度的概念,利用振动来诱导和探测科里奥利力。
MEMS陀螺仪的核心是一个微加工机械单元,在设计上按照一个音叉机制共振运动,通过科里奥利力原理把角速率转换成一个特定感测结构的位移。

在这里插入图片描述

两个相同的质量块以方向相反的做水平震荡。当外部施加一个角速率,就会出现一个科氏力,力的方向垂直于质量运动方向,如垂直方向箭头所示。产生的科氏力使感测质量发生位移,位移大小与所施加的角速率大小成正比,科氏力引起的电容变化即可计算出角速率大小。
科里奥利效应指出,当质量 (m) 以速度 (v) 沿特定方向移动并施加外部角速率 (Ω)(红色箭头)时,科里奥利效应会产生一个力(黄色箭头),导致质量垂直移动。该位移的值与应用的角速率直接相关。

在这里插入图片描述

  /* USER CODE BEGIN 2 */printf("123123123");lsm6dsv16x_reset_t rst;stmdev_ctx_t dev_ctx;/* Initialize mems driver interface */dev_ctx.write_reg = platform_write;dev_ctx.read_reg = platform_read;dev_ctx.handle = &SENSOR_BUS;HAL_GPIO_WritePin(CS_GPIO_Port, CS_Pin, GPIO_PIN_SET);HAL_GPIO_WritePin(SA0_GPIO_Port, SA0_Pin, GPIO_PIN_RESET);/* Wait sensor boot time */platform_delay(BOOT_TIME);/* Check device ID */lsm6dsv16x_device_id_get(&dev_ctx, &whoamI);printf("LSM6DSV16X_ID=0x%x,whoamI=0x%x",LSM6DSV16X_ID,whoamI);if (whoamI != LSM6DSV16X_ID)while (1);/* Restore default configuration */lsm6dsv16x_reset_set(&dev_ctx, LSM6DSV16X_RESTORE_CTRL_REGS);do {lsm6dsv16x_reset_get(&dev_ctx, &rst);} while (rst != LSM6DSV16X_READY);/* Enable Block Data Update */lsm6dsv16x_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);/* Set Output Data Rate.* Selected data rate have to be equal or greater with respect* with MLC data rate.*/lsm6dsv16x_xl_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_960Hz);lsm6dsv16x_gy_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_960Hz);/* Set full scale */lsm6dsv16x_xl_full_scale_set(&dev_ctx, LSM6DSV16X_2g);lsm6dsv16x_gy_full_scale_set(&dev_ctx, LSM6DSV16X_2000dps);/* Configure filtering chain */filt_settling_mask.drdy = PROPERTY_ENABLE;filt_settling_mask.irq_xl = PROPERTY_ENABLE;filt_settling_mask.irq_g = PROPERTY_ENABLE;lsm6dsv16x_filt_settling_mask_set(&dev_ctx, filt_settling_mask);lsm6dsv16x_filt_gy_lp1_set(&dev_ctx, PROPERTY_ENABLE);lsm6dsv16x_filt_gy_lp1_bandwidth_set(&dev_ctx, LSM6DSV16X_GY_ULTRA_LIGHT);lsm6dsv16x_filt_xl_lp2_set(&dev_ctx, PROPERTY_ENABLE);lsm6dsv16x_filt_xl_lp2_bandwidth_set(&dev_ctx, LSM6DSV16X_XL_STRONG);int16_t	acc_int16[3]	={0,0,0};int16_t	gyr_int16[3]		={0,0,0};	float acc[3] = {0};float gyr[3] = {0};	uint8_t data[21]={0};data[0]=0xAB;//帧头data[1]=0xFD;//源地址data[2]=0xFE;//目标地址		data[3]=0x01;//功能码ID	data[4]=0x0D;//数据长度LENdata[5]=0x00;//数据长度LEN 13uint8_t sumcheck = 0;
uint8_t addcheck = 0;		int16_t angular_rate_raw[3]={0,0,0};	//pitch,roll,yawuint8_t data_angular_rate_raw[16]={0};data_angular_rate_raw[0]=0xAB;//帧头data_angular_rate_raw[1]=0xFD;//源地址data_angular_rate_raw[2]=0xFE;//目标地址		data_angular_rate_raw[3]=0x03;//功能码ID	data_angular_rate_raw[4]=0x08;//数据长度LENdata_angular_rate_raw[5]=0x00;//数据长度LEN 8data_angular_rate_raw[6]=0x01;//mode = 1	data_angular_rate_raw[13]=0x00;//FUSION _STA:融合状态	/* USER CODE END 2 */

主程序

  /* Infinite loop *//* USER CODE BEGIN WHILE */while (1){lsm6dsv16x_data_ready_t drdy;/* Read output only if new xl value is available */lsm6dsv16x_flag_data_ready_get(&dev_ctx, &drdy);if (drdy.drdy_xl) {/* Read acceleration field data */memset(data_raw_acceleration, 0x00, 3 * sizeof(int16_t));lsm6dsv16x_acceleration_raw_get(&dev_ctx, data_raw_acceleration);acceleration_mg[0] =lsm6dsv16x_from_fs2_to_mg(data_raw_acceleration[0]);acceleration_mg[1] =lsm6dsv16x_from_fs2_to_mg(data_raw_acceleration[1]);acceleration_mg[2] =lsm6dsv16x_from_fs2_to_mg(data_raw_acceleration[2]);
//			printf("Acceleration [mg]:%4.2f\t%4.2f\t%4.2f\r\n",acceleration_mg[0], acceleration_mg[1], acceleration_mg[2]);}		/* Read output only if new xl value is available */if (drdy.drdy_gy) {/* Read angular rate field data */memset(data_raw_angular_rate, 0x00, 3 * sizeof(int16_t));lsm6dsv16x_angular_rate_raw_get(&dev_ctx, data_raw_angular_rate);angular_rate_mdps[0] =lsm6dsv16x_from_fs2000_to_mdps(data_raw_angular_rate[0]);angular_rate_mdps[1] =lsm6dsv16x_from_fs2000_to_mdps(data_raw_angular_rate[1]);angular_rate_mdps[2] =lsm6dsv16x_from_fs2000_to_mdps(data_raw_angular_rate[2]);
//			printf("Angular rate [mdps]:%4.2f\t%4.2f\t%4.2f\r\n",angular_rate_mdps[0], angular_rate_mdps[1], angular_rate_mdps[2]);}if (drdy.drdy_temp) {/* Read temperature data */memset(&data_raw_temperature, 0x00, sizeof(int16_t));lsm6dsv16x_temperature_raw_get(&dev_ctx, &data_raw_temperature);temperature_degC = lsm6dsv16x_from_lsb_to_celsius(data_raw_temperature);
//			printf("Temperature [degC]:%6.2f\r\n", temperature_degC);}		IMUupdate(angular_rate_mdps[0]/1000,angular_rate_mdps[1]/1000,angular_rate_mdps[2]/1000,acceleration_mg[0]/1000,acceleration_mg[1]/1000,acceleration_mg[2]/1000);
Roll=Roll*100;
Pitch=Pitch*100;
Yaw=Yaw*100;
data_angular_rate_raw[8]=(int16_t)Roll>>8;//roll
data_angular_rate_raw[7]=(int16_t)Roll;
data_angular_rate_raw[10]=(int16_t)Pitch>>8;//pitch
data_angular_rate_raw[9]=(int16_t)Pitch;
data_angular_rate_raw[12]=(int16_t)Yaw>>8;//yaw
data_angular_rate_raw[11]=(int16_t)Yaw;
sumcheck = 0;
addcheck = 0;
for(uint16_t i=0; i < 14; i++)
{
sumcheck += data_angular_rate_raw[i]; //从帧头开始,对每一字节进行求和,直到 DATA 区结束
addcheck += sumcheck; //每一字节的求和操作,进行一次 sumcheck 的累加
}
data_angular_rate_raw[14]=sumcheck;
data_angular_rate_raw[15]=addcheck;HAL_UART_Transmit(&huart1 , (uint8_t *)&data_angular_rate_raw, 16, 0xFFFF);
//printf("Roll=%.2f,Pitch=%.2f,Yaw=%.2f\n",Roll,Pitch,Yaw);HAL_Delay(100);/* USER CODE END WHILE *//* USER CODE BEGIN 3 */}/* USER CODE END 3 */

演示

上报匿名助手能正常进行解析。

在这里插入图片描述

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

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

多人聊天室

多人聊天包 由于要先创建服务面板&#xff0c;接收客户端连接的信息&#xff0c;此代码使用顺序为先启动服务端&#xff0c;在启动客户端&#xff0c;服务端不用关&#xff0c;不然会报错。多运行几次客户端&#xff0c;实现单人聊天 1.创建服务面板 package yiduiy;import j…

【计算机二级MS Office】word(上)

这里写目录标题 文件选项卡保存和另存为属性检查文档 开始选项卡字体更改字体和字号设置中文和英文为两种不同字体的快捷方式介绍其余图标文本效果突出颜色如何挑选字体颜色字符底纹带圈字符字体对话框&#xff08;隐藏&#xff09; 段落 插入选项卡设计选项卡布局选项卡引用选…

【头歌系统数据库实验】实验6 SQL的多表查询-2

目录 第1关&#xff1a;查询每个选手的信息及其提交的解答信息&#xff0c;没做题的选手不显示 第2关&#xff1a;查询做了1001题且耗时大于500&#xff08;time&#xff09;的选手信息 第3关&#xff1a;查询所有选手信息及其提交的解答信息&#xff0c;没做题的选手也要显…

力扣每日一题:2646. 最小化旅行的价格总和(2023-12-06)

力扣每日一题 题目&#xff1a;2646. 最小化旅行的价格总和 日期&#xff1a;2023-12-06 用时&#xff1a;30 m 14 s 时间&#xff1a;8ms 内存&#xff1a;42.98MB 思路&#xff1a;先统计旅行中每个节点路过的次数&#xff08;dfs方法&#xff09;&#xff0c;再计算减半后的…

项目中使用之Maven BOM

✅作者简介&#xff1a;大家好&#xff0c;我是Leo&#xff0c;热爱Java后端开发者&#xff0c;一个想要与大家共同进步的男人&#x1f609;&#x1f609; &#x1f34e;个人主页&#xff1a;Leo的博客 &#x1f49e;当前专栏&#xff1a; 工具教程 ✨特色专栏&#xff1a; MyS…

Linux文件部分知识

目录 认识inode 如何理解创建一个空文件&#xff1f; 如何理解对文件写入信息&#xff1f; 如何理解删除一个文件&#xff1f; 为什么拷贝文件的时候很慢&#xff0c;而删除文件的时候很快&#xff1f; 如何理解目录 ​编辑 文件的三个时间 ​编辑 Access&#xff1a; …

Linux系统调试课:网络性能工具总结

文章目录 一、网络性能指标二、netstat三、route四、iptables沉淀、分享、成长,让自己和他人都能有所收获!😄 📢本篇章一起了解下网络性能工具。 一、网络性能指标 从网络性能指标出发,你更容易把性能工具同系统工作原理关联起来,对性能问题有宏观的认识和把握。这样,…

【LeetCode刷题-链表】--92.反转链表II

92.反转链表II /*** Definition for singly-linked list.* public class ListNode {* int val;* ListNode next;* ListNode() {}* ListNode(int val) { this.val val; }* ListNode(int val, ListNode next) { this.val val; this.next next; }* }*/ cla…

前端JavaScript入门-day08-正则表达式

目录 介绍 语法 元字符 边界符 量词 字符类&#xff1a; 修饰符 介绍 正则表达式&#xff08;Regular Expression&#xff09;是用于匹配字符串中字符组合的模式。在 JavaScript中&#xff0c;正则表达式也是对象&#xff0c;通常用来查找、替换那些符合正则表达式的…

书-用数组存储高于60低于70的人单独存起来

#include<stdio.h> # define N 10 //书-用数组存储高于60低于70的人单独存起来 int main(){float s[N]{68.2,62.3,63.4,34.5,45.6,56.7,67.8,78.9,89.0,100};int i;float diyu[100];int j0;for(i0;i<N;i){if(s[i]>60 && s[i]<70)diyu[j]s[i];//这里的范…

【数据结构】——二叉树特点

前言&#xff1a;我们前面已经了解了二叉树的一些概念&#xff0c;那么我们今天就来了解下二叉树的遍历实现和一些性质。 二叉树的遍历方式有三种&#xff1a;前序&#xff0c;中序&#xff0c;后序。 前序&#xff1a;先根节点&#xff0c;再左子树&#xff0c;最后右子树。 中…

绘制6层及以上PCB板,需要明白PCB板的结构和叠层

PCB主要由PP半固化片和core芯板压合而成&#xff0c;其中core芯板两面都有铜箔&#xff0c;是PCB板的导电介质&#xff1b;PP半固化片是绝缘材料&#xff0c;用于芯板的粘合。 在PP半固化片被层压后&#xff0c;其环氧树脂被挤压开来&#xff0c;将core芯板粘合在一起。 PCB的叠…

Python代码将txt里面多行json字符串转成excel文件

python 代码 将txt里面的多行json字符串转成excel history.txt文件json代码样例 Json转换Excel代码 import json import pandas as pddef json_out(file_path,excel_path):all_list[]with open(file_path, "r", encodingutf-8) as f:for line in f:all_list.append…

Tuxera NTFS2024安装包下载教程

在听到小凡的电话说“Tuxera NTFS for Mac软件安装失败&#xff0c;怎么办”的时候&#xff0c;小编心里真像有一万头草泥马在奔腾——苹果软件还能安装失败&#xff01;&#xff1f; 挥手把一万头草泥马赶走&#xff0c;脑补着苹果软件的安装&#xff0c;小编对着电话吼道&am…

配置主机与外网时间服务器同步时间

目录 NTP服务简介 时间管理命令 第一步&#xff1a;更改当前主机所在地的时间 方法一&#xff1a;使用tzselect命令查询需要的时区 1、使用tzselect命令查询需要的时区 2、添加变量到 ~/.bash_profile 文件中&#xff0c;即追加类似的内容&#xff1a; 3、重新连接一个…

LLM大语言模型(一):ChatGLM3-6B本地部署

目录 前言 本机环境 ChatGLM3代码库下载 模型文件下载 修改为从本地模型文件启动 启动模型网页版对话demo 超参数设置 GPU资源使用情况 &#xff08;网页对话非常流畅&#xff09; 前言 LLM大语言模型工程化&#xff0c;在本地搭建一套开源的LLM&#xff0c;方便后续的…

深入了解UUID:生成、应用与优势

一、引言 在当今数字化时代&#xff0c;唯一标识一个对象的能力变得越来越重要。UUID&#xff08;Universally Unique Identifier&#xff0c;通用唯一标识符&#xff09;应运而生&#xff0c;作为一种保证全球唯一性的标识方法&#xff0c;广泛应用于各种场景。本文将详细介绍…

HTML程序大全(2):通用注册模版

一、正常情况效果 二、某项没有填写的效果 三、没有勾选同意项的效果 四、代码 <!DOCTYPE html> <html> <head><meta charset"UTF-8"><title>注册</title><style>body {font-family: Arial, sans-serif;background-color…

21章网络通信

21.1——网络程序设计基础 网络程序设计编写得到是与其他计算机进行通信的程序 21.1.1——局域网与互联网 为了实现两台计算机的通信&#xff0c;必须用一个网络线路连接两台计算机 21.1.2——网络协议 网络协议规定了计算机之间连接的物理、机械 (网线与网卡的连接规定)、…

「智慧城市」这一概念科学吗?还是炒作?

智慧城市是一个综合性的概念&#xff0c;它利用信息技术和创新概念&#xff0c;将城市的各个系统和服务集成起来&#xff0c;以提升城市运行效率、优化城市管理和服务&#xff0c;改善市民的生活质量。 具体来说&#xff0c;智慧城市涵盖了许多领域&#xff0c;包括城市规划、建…