matlab入門

clear
scan = blelist("Timeout", 20);
Nano33BLE_address="88A2EDC9F60C";  % scanで見つかったアドレスを代入
b = ble(Nano33BLE_address);

% LSM9DS1

ServiceUUID    = "F000AE00-0451-4000-B000-000000000000";
LSM9DS1_Acceleration_Characteristic_UUID  =  "F000AE01-0451-4000-B000-000000000000";
LSM9DS1_Gyroscope_Characteristic_UUID     =  "F000AE02-0451-4000-B000-000000000000";
LSM9DS1_MagneticField_Characteristic_UUID =  "F000AE03-0451-4000-B000-000000000000";


g11 = characteristic(b, ServiceUUID, LSM9DS1_Acceleration_Characteristic_UUID); % 配列
subscribe(g11);  % Notify
g12 = characteristic(b, ServiceUUID, LSM9DS1_Gyroscope_Characteristic_UUID);
subscribe(g12);
g13 = characteristic(b, ServiceUUID, LSM9DS1_MagneticField_Characteristic_UUID);
subscribe(g13);

[Acceleration, Gyroscope, MagneticField] = read_IMU(g11, g12, g13);
% fprintf('\n Acceleration(g) is %.2f %.2f %.2f ', Acceleration(:,1),Acceleration(:,2),Acceleration(:,3));
% fprintf('\n Gyroscope(Hz) is %.2f %.2f %.2f ', Gyroscope(:,1),Gyroscope(:,2),Gyroscope(:,3));
% fprintf('\n Magnetic Field(uT) is %.2f %.2f %.2f ', MagneticField(:,1),MagneticField(:,2),MagneticField(:,3));

tic;
stopTimer = 10;  % first 100
magReadings=[1:stopTimer;1:stopTimer;1:stopTimer]';

while(toc<stopTimer)
    % Rotate the sensor around x axis from 0 to 360 degree.
    % Take 2-3 rotations to improve accuracy.
    % For other axes, rotate around that axis.
    % [accel, Gyroscope, mag] = read_IMU(g11, g12, g13);
    [magReadings] = read_IMU(g11, g12, g13);
end

% For y axis, use magReadings (:,2) and for z axis use magReadings(:,3);
magx_min = min(magReadings(:,1));
magx_max = max(magReadings(:,1));
magx_correction = (magx_max+magx_min)/2;
    
% GyroscopeNoise and AccelerometerNoise is determined from datasheet.
GyroscopeNoiselsm9ds1 = 3.0462e-06; % GyroscopeNoise (variance value) in units of rad/s
AccelerometerNoiselsm9ds1 = 0.0061; % AccelerometerNoise(variance value)in units of m/s^2
viewer = HelperOrientationViewer('Title',{'AHRS Filter'});
% FUSE = ahrsfilter('SampleRate',b.SampleRate, 'GyroscopeNoise',GyroscopeNoiselsm9ds1,'AccelerometerNoise',AccelerometerNoiselsm9ds1);
FUSE = ahrsfilter('GyroscopeNoise',GyroscopeNoiselsm9ds1,'AccelerometerNoise',AccelerometerNoiselsm9ds1);

stopTimer = 200;

% Use ahrsfilter to estimate orientation and update the viewer as the
% sensor moves for time specified by stopTimer
disp('start');
tic;
accelReadings = [1:stopTimer;1:stopTimer;1:stopTimer]';
gyroReadings  = [1:stopTimer;1:stopTimer;1:stopTimer]';
magneticReading=[1:stopTimer;1:stopTimer;1:stopTimer]';
while(toc < stopTimer)
    [accelReadings, gyroReadings, magneticReading] = read_IMU(g11, g12, g13);
    pause(0.1);
    rotators = FUSE(accelReadings, gyroReadings, magneticReading);
    poseplot(rotators, MeshFileName="plane3.stl");
    view([50 -50 10]);
end

unsubscribe(g11);
unsubscribe(g12);
unsubscribe(g13);
clear b



function [Acceleration, Gyroscope, MagneticField] = read_IMU(g11, g12, g13)
    accelerationXYZ = char(read(g11));
    dataA = split(accelerationXYZ, ",");
    aX = dataA{1,:};
    aY = dataA{2,:};
    aZ = dataA{3,:};
    Acceleration = [str2double(aX)*9.8 str2double(aY)*9.8 str2double(aZ)*9.8];
    % fprintf('\n Acceleration(g) is %s %s %s ', aX,aY,aZ);

    gyroscopeXYZ = char(read(g12));
    dataG = split(gyroscopeXYZ, ",");
    gX = dataG{1,:};
    gY = dataG{2,:};
    gZ = dataG{3,:};
    Gyroscope = [str2double(gX), str2double(gY), str2double(gZ)];
    % fprintf('\n Gyroscope(Hz) is %s %s %s ', gX,gY,gZ);

    magneticFieldXYZ = char(read(g13));
    dataM = split(magneticFieldXYZ, ",");
    mX = dataM{1,:};
    mY = dataM{2,:};
    mZ = dataM{3,:};
    MagneticField = [str2double(mX), str2double(mY), str2double(mZ)];
    % fprintf('\n Magnetic Field(uT) is %s %s %s ', mX,mY,mZ);
    % disp("-");
    % pause(2)
end