%完成选择一个空的坐标轴、、、、202108301530
%完成添加串口，读取yaw,pitch,roll三个角度，让坐标旋转;
%暂时做一个轴的三个角度，帧格式：0x21,yaw,pitch,roll,0x0D,0x0A
% Create a 3-D plot
clc
clear
close all
global RecData0;
RecData0=0;
global RecCount;%串口接收数据的数量，帧头等使用
RecCount=0;
ax = axes('XLim', [-5 5], 'YLim', [-5 5], 'ZLim', [-5 5]);
xlabel(ax, 'X-axis');
ylabel(ax, 'Y-axis');
zlabel(ax, 'Z-axis');
% % Reverse the 2 axis directions to match the device coordinate system
%set(ax, 'Zdir', 'reverse');
%set(ax, 'Ydir', 'reverse');
grid on;
view(3);%看三维的视图
% Define the surface color
color = [0.3010 0.7450 0.9330];
%用caxa画图，然后读出坐标。这里用薄片，因此z轴坐标等于0，分别是每个点的x,y,z坐标
markdiy=[3 0 0;
1 1 0;
1 3 0;
-1 3 0;
-1 1 0;
-3 1 0;
-3 2 0;
-3.5 2 0;
-3.5 -2 0;
-3 -2 0;
-3 -1 0;
-1 -1 0;
-1 -3 0;
1 -3 0;
1 -1 0;
];
%构建薄片，分别是xi，yi,zi坐标markdiy(:,1)，是markdiy的第一个数，就是x,坐标
p(1) = patch(markdiy(:,1), markdiy(:,2),markdiy(:,3),'yellow');
mark0 = [0.2 -0.2 0.2; 2 0 0.2; 0.2 0.2 0.2];
p(2) = patch(mark0(:,1), mark0(:,2), mark0(:,3), 'red');
mark1 = [0.2 0.2 0.2; 0 2 0.2; -0.2 0.2 0.2];
p(3) = patch(mark1(:,1), mark1(:,2), mark1(:,3), 'green');
mark2 = [0 0.2 0; 0 0 2; 0 -0.2 0];
p(4) = patch(mark2(:,1), mark2(:,2), mark2(:,3), 'blue');
% 设置透明度
alpha(0.9)
%After the 3-D object is created, link the rotation matrix data acquired from the device to
%the plot by using hgtransform.
%将轴和3D物体的坐标系
tfObject = hgtransform('Parent', ax);
set(p, 'Parent', tfObject);
pause(5);
% %**************************************
freeports = serialportlist("available") %列出空闲串口列表
seri= serialport("COM4",115200);
%串口格式
seri.Parity="none";
seri.StopBits=1;
seri.DataBits=8;
seri.ByteOrder="little-endia";%stm32默认是小端模式
%检查终止符检查默认的 ASCII 终止符。
%注意，一般LF是换行，就是\n,所以在stm32端发送数据之后，需要再发送一个\n；
%也可以更改configureTerminator(device,"CR")，CR对应\r回车
configureTerminator(seri,"CR/LF")
seri.Terminator
flush(seri);
%---Prepare the UserData property to store the Arduino data.
%---The Data field of the struct saves the sine wave value and the Count field saves the x-axis value of the sine wave.
seri.UserData = struct("Data",[],"Count",1);
seri.BytesAvailableFcnMode
pause(2);
%必须将这个tic放在这里，前面搜索串口等会花费时间，在这里开始计算时间画图
seri.BytesAvailableFcnMode
yaw=0;
pitch=0;
roll=0;
global yawtemp;
global pitchtemp;
global rolltemp;
yawtemp=0;
pitchtemp=0;
rolltemp=0;
flightTime =5;
configureCallback(seri, "off");
seri.BytesAvailableFcnMode
disp("开始发送数据吧...");
pause(5);
disp("开始发送数据吧...");
pause(2);
disp("okokokok...");
pause(2);
configureCallback(seri,"terminator",@readSerialData);
tObj = tic;
while( floor(toc(tObj)) < flightTime)
disp(toc(tObj));
pause(0.2)
end
seri.BytesAvailableFcnMode %检查是不是关闭了回调函数，可以删掉这行
%将数据保存到diydata文件中，diydata在当前目录里面
save diydata yawtemp pitchtemp rolltemp
%关回调函数
configureCallback(seri, "off");
disp("******************")
disp(toc(tObj))
flush(seri);
clear seri
pause(1)
%更新三个轴的数据，串口进来的数据进行更新
for i=1:numel(yawtemp)
yaw=yawtemp(i);
pitch=pitchtemp(i);
roll=rolltemp(i);
%生成旋转矩阵myRotationMatrix,3x3
q=quaternion([yaw pitch roll],'eulerd','zyx','frame');
myRotationMatrix=rotmat(q,'frame');
%生成要绑定的变换矩阵4x4
transformMatrix = eye(4);
%将myRotationMatrix放入到transformMatrix中，
%前面的上三角3x3矩阵是旋转矩阵
for i=1:3
for j=1:3
transformMatrix(i,j)=myRotationMatrix(i,j);
end
end
%将旋转矩阵用于物体，得到旋转
set(tfObject, 'Matrix', transformMatrix);
drawnow
pause(0.2);
end
clear
%**********************************************************
%进来是一个字节进行回调，比较浪费时间。？这了用回车换行作为结束符回调
%在回车换行前，取结束符前四个，作为yaw,以此类推，得到三个角度
%这里需要知道接收的长度
function readSerialData(src, ~)
global yawtemp;
global pitchtemp;
global rolltemp;
data = readline(src);
yawtemp(end+1)= str2double(extractBetween(data,1,7));
pitchtemp(end+1)= str2double(extractBetween(data,9,15));
rolltemp(end+1)= str2double(extractBetween(data,17,23));
disp(yawtemp(1:end))
disp(pitchtemp(1:end))
disp(rolltemp(1:end))
disp("------------=====");
end
stm32端代码：
#include "./stm32f4xx_it.h"
#include "./SYSTEM/sys/sys.h"
#include "./SYSTEM/usart/usart.h"
#include "./SYSTEM/delay/delay.h"
#include "./BSP/LED/led.h"
#include "./BSP/USART2/usart2.h"
#include "./IMU901/imu901.h"
int main(void)
{
uint32_t times = 0;
uint8_t ch;
float xx=10.5f,yy=25.5f,zz=80.2f;
float zztemp=2;
HAL_Init();
sys_stm32_clock_init();
delay_init(168);
usart_init(115200);
led_init();
usart2_init(115200);
imu901_init();
while (1)
{
if (imu901_uart_receive(&ch, 1))
{
if (imu901_unpack(ch))
{
if (rxPacket.startByte2 == UP_BYTE2)
{
atkpParsing(&rxPacket);
}
}
}
else
{
delay_ms(1);
times++;
if (times % 300 == 0) LED0_TOGGLE();
if (times % 200 == 0)
{
zz=10.2+zztemp;
yy=0;
xx=0;
printf("%07.2f,%07.2f,%07.2f",zz,yy,xx);
printf("\r\n");
zztemp=zztemp+0.5;
}
}
}
}
注意，这里的%07.2f意思是7位宽度，包括了小数点以及小数点后面的数字，总的是七位，不够七位e 前面补0补够，超过7位的按照实际 长度进行输出。这里是三个角度，最多是-365.35总共7位，所以这里设置7位，就足够了。看实际情况进行解决。
``` printf("%07.2f,%07.2f,%07.2f",zz,yy,xx);
matlab用回车换行进行触发回调函数。
回调函数中，读取数据。分别放入三个全局变量中。
这里注意，函数shi读取数据，保存为文件中，然后将文件的数据拿出来nx 绘图。
而不是实时绘图。
需要实时描图需要稍微做更改。
|