Nano 33 BLE Senseをデータ入力に使う＜BLE>⑬9軸IMU LSM9DS1＜BLEセントラル その2＞
連載の10回では、9軸慣性センサ LSM9DS1のBLEペリフェラルを受けるために、BLEのセントラルをmatlabで記述しました。また、
Arduino MKR WiFi 1010をデータ入力に使う⑬I2C 9軸IMU LSM9DS1
では、9軸のデータを読み取り、3D表示を行いました。
ここでは、BLEで9軸のデータを受信し、3D表示を行います。
3Dの表示は、
Estimating Orientation Using Inertial Sensor Fusion and MPU-9250
を参照しました。この表示には、Sensor Fusion and Tracking ToolboxもしくはNavigation Toolboxが必要です。ホーム・ライセンスではNavigation Toolboxが購入できます。
●BLE受信部分
最初は、ほかのセントラルと同じ記述がつづきます。
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));
9軸のデータを取得する部分はFunctionに分離しました。
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
●地磁気の処理
次のルーチンは、最初stopTimer = 100;で実行します。ボードを水平に持ち。8の字型に方向をずらします。北の方向が確定します。
一度行えばいいはずなので、二度目以降は、stopTimer = 10;にして、すぐ通過できるように修正します。
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;
ahrsfilterは、加速度計、ジャイロスコープ、磁力計のセンサ・データを融合して、デバイスの向きと角速度を推定するための間接カルマン・フィルタ・システム・オブジェクトを返します。フィルタは、12要素の状態ベクトルを使用して、方向、ジャイロスコープのバイアス、線形加速度、および磁気擾乱の推定誤差を追跡します。
このプログラムを開始する前に、センサは静止している必要があります。二つのノイズ量を記述していますが、適切ではないと思われます。求め方は現時点で不明です。
SampleRateは、BLEでは不要なので削除しました。
% 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);
●3D表示部分
次のプログラムは、図形を200カウントの間、ボードの方向を読み取って、中央の四角い物体を動かし続けます。
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);
for j = numel(rotators)
viewer(rotators(j));
pause(0.1);
end
end
全体のプログラムです。
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);
for j = numel(rotators)
viewer(rotators(j));
pause(0.1);
end
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
