SPRESENSEでクォータニオンを出す

加速度と角速度が出せたので、これでクォータニオンを出してみる。(地磁気センサがあれば、絶対姿勢が出せるのだが、今回はおあずけ)
センサフュージョンのアルゴリズムには、Madgwick Orientation Filterを使ってみた。論文の最後にあるCのコード(IMU Filter)を使用した。

#include <BMI160Gen.h>
// Math library required for ‘sqrt’
#include <math.h>
// System constants
define deltat 0.005f // 5msec sampling
define gyroMeasError 3.14159265358979f * (5.0f / 180.0f)
define beta sqrt(3.0f / 4.0f) * gyroMeasError
// Global system variables
float SEq_1 = 1.0f, SEq_2 = 0.0f, SEq_3 = 0.0f, SEq_4 = 0.0f;
int count = 0;
void setup() {
Serial.begin(9600); // initialize Serial communication
while (!Serial); // wait for the serial port to open
// initialize device
Serial.println("Initializing IMU device…");
//BMI160.begin(BMI160GenClass::SPI_MODE, /* SS pin# = */10);
BMI160.begin(BMI160GenClass::I2C_MODE);
uint8_t dev_id = BMI160.getDeviceID();
Serial.print("DEVICE ID: ");
Serial.println(dev_id, HEX);
// Set the accelerometer range to 250 degrees/second
BMI160.setGyroRange(250);
BMI160.setAccelerometerRange(2);
Serial.println("Initializing IMU device…done.");
}
void loop() {
int gxRaw, gyRaw, gzRaw; // raw gyro values
float gx, gy, gz;
int axRaw, ayRaw, azRaw; // raw acc values
float ax, ay, az;
// read raw gyro measurements from device
BMI160.readGyro(gxRaw, gyRaw, gzRaw);
// read raw acc
BMI160.readAccelerometer(axRaw, ayRaw, azRaw);
// convert the raw gyro data to degrees/second
gx = convertRawGyro(gxRaw);
gy = convertRawGyro(gyRaw);
gz = convertRawGyro(gzRaw);
// convert the raw acc data to G
ax = convertRawAcc(axRaw);
ay = convertRawAcc(ayRaw);
az = convertRawAcc(azRaw);
filterUpdate(gx, gy, gz, ax, ay, az);
count++;
if(count >= 100){
// display tab-separated q values
Serial.print("q:\t");
Serial.print(SEq_1);
Serial.print("\t");
Serial.print(SEq_2);
Serial.print("\t");
Serial.print(SEq_3);
Serial.print("\t");
Serial.print(SEq_4);
Serial.println();
count = 0;
}
delay(5); // 5msec sampling
}
float convertRawGyro(int gRaw) {
// since we are using 250 degrees/seconds range
// -250 maps to a raw value of -32768
// +250 maps to a raw value of 32767
float g = (gRaw * 250.0) / 32768.0;
g = g / 180.0f * 3.14159265358979f;
return g;
}
float convertRawAcc(int aRaw){
// since we are using 2G range
// -2G maps to a raw value of -32768
// +2G maps to a raw value of 32767
float a = (aRaw * 2.0) / 32768.0;
return a;
}
void filterUpdate(float w_x, float w_y, float w_z, float a_x, float a_y, float a_z){
// Local system variables
float norm;
float SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4;
float f_1, f_2, f_3;
float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33;
float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4;
float halfSEq_1 = 0.5f * SEq_1;
float halfSEq_2 = 0.5f * SEq_2;
float halfSEq_3 = 0.5f * SEq_3;
float halfSEq_4 = 0.5f * SEq_4;
float twoSEq_1 = 2.0f * SEq_1;
float twoSEq_2 = 2.0f * SEq_2;
float twoSEq_3 = 2.0f * SEq_3;
// Normalise the accelerometer measurement
if(a_x == 0.0f && a_y == 0.0f && a_z == 0.0f) a_x = 1.0f;
norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
a_x /= norm;
a_y /= norm;
a_z /= norm;
// Compute the objective function and Jacobian
f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x;
f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y;
f_3 = 1.0f - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z;
J_11or24 = twoSEq_3;
J_12or23 = 2.0f * SEq_4;
J_13or22 = twoSEq_1;
J_14or21 = twoSEq_2;
J_32 = 2.0f * J_14or21;
J_33 = 2.0f * J_11or24;
// Compute the gradient (matrix multiplication)
SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1;
SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3;
SEqHatDot_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1;
SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2;
// Normalise the gradient
norm = sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);
SEqHatDot_1 /= norm;
SEqHatDot_2 /= norm;
SEqHatDot_3 /= norm;
SEqHatDot_4 /= norm;
// Compute the quaternion derrivative measured by gyroscopes
SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z;
SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y;
SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x;
SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x;
// Compute then integrate the estimated quaternion derrivative
SEq_1 += (SEqDot_omega_1 - (beta * SEqHatDot_1)) * deltat;
SEq_2 += (SEqDot_omega_2 - (beta * SEqHatDot_2)) * deltat;
SEq_3 += (SEqDot_omega_3 - (beta * SEqHatDot_3)) * deltat;
SEq_4 += (SEqDot_omega_4 - (beta * SEqHatDot_4)) * deltat;
// Normalise quaternion
norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
SEq_1 /= norm;
SEq_2 /= norm;
SEq_3 /= norm;
SEq_4 /= norm;
}

SPRESENSE購入から、加速度・角速度の表示まで

SPRESENSEを購入した。購入先はスイッチサイエンス。SPRESENSEメインボード、SPRESENSE拡張ボード、SPRESENSE用3軸加速度・3軸ジャイロ・気圧・温度センサ アドオンボードの3点。

SPRESENSEの基本の使い方は、ここに出ている。この情報で、LEDを点滅させるところまで問題なくできた。

次に、購入したアドオンボードのBMI160に入っている加速度・角速度センサを使ってみることにした。ドライバは、ここのものを使用した。

以下、加速度と角速度を出力するコード。

#include <BMI160Gen.h>
void setup() {
Serial.begin(9600); // initialize Serial communication
while (!Serial); // wait for the serial port to open
// initialize device
Serial.println("Initializing IMU device…");
//BMI160.begin(BMI160GenClass::SPI_MODE, /* SS pin# = */10);
BMI160.begin(BMI160GenClass::I2C_MODE);
uint8_t dev_id = BMI160.getDeviceID();
Serial.print("DEVICE ID: ");
Serial.println(dev_id, HEX);
// Set the accelerometer range to 250 degrees/second
BMI160.setGyroRange(250);
BMI160.setAccelerometerRange(2);
Serial.println("Initializing IMU device…done.");
}
void loop() {
int gxRaw, gyRaw, gzRaw; // raw gyro values
float gx, gy, gz;
int axRaw, ayRaw, azRaw; // raw acc values
float ax, ay, az;
// read raw gyro measurements from device
BMI160.readGyro(gxRaw, gyRaw, gzRaw);
// read raw acc
BMI160.readAccelerometer(axRaw, ayRaw, azRaw);
// convert the raw gyro data to degrees/second
gx = convertRawGyro(gxRaw);
gy = convertRawGyro(gyRaw);
gz = convertRawGyro(gzRaw);
// convert the raw acc data to G
ax = convertRawAcc(axRaw);
ay = convertRawAcc(ayRaw);
az = convertRawAcc(azRaw);
// display tab-separated gyro x/y/z acc x/y/z values
Serial.print("g:\t");
Serial.print(gx);
Serial.print("\t");
Serial.print(gy);
Serial.print("\t");
Serial.print(gz);
Serial.print("\t");
Serial.print("a:\t");
Serial.print(ax);
Serial.print("\t");
Serial.print(ay);
Serial.print("\t");
Serial.print(az);
Serial.println();
delay(500);
}
float convertRawGyro(int gRaw) {
// since we are using 250 degrees/seconds range
// -250 maps to a raw value of -32768
// +250 maps to a raw value of 32767
float g = (gRaw * 250.0) / 32768.0;
return g;
}
float convertRawAcc(int aRaw){
// since we are using 2G range
// -2G maps to a raw value of -32768
// +2G maps to a raw value of 32767
float a = (aRaw * 2.0) / 32768.0;
return a;
}

ちなみに、MACでSerialを表示させるには、以下のコマンドを使用した。

sudo cu -l /dev/tty.SLAB_USBtoUART


ZenBook3のバッテリー交換

ZenBook3のバッテリー膨張により、キーボードが膨らんで、閉まらなくなってしまった件、無償修理してもらえることになった。稀なバッテリー不具合だとのこと。稀ってことは、運悪くハズレを引いてしまったのかな。ちなみに、ASUSのサポートセンターとは、メールでやりとりしたが、対応も早くいい感じだった。やっぱり製品のサポート業務って重要だなと思った。

モバイル機器のバッテリーが膨らむ

ここのところ、所有しているモバイル機器のバッテリーが膨らんでしまうことが多い。特に、暖かいところに置いておくとバッテリーが膨らむことがある模様。
まず、ZenPadは、SETI@homeの計算を走らせていたこともあり、いつも暖かい状態だった。そのこともあり、本体が膨らんでしまった。完全にプラスチック部分が変形してしまったため、これは修理は大変だなと思い、廃棄処分とした。
次に、iPhone5s。PCのACアダプタの上に乗せていたら、温まってしまい、バッテリーが膨らんで、パッカリ開いてしまった。これも修理は大変だなと思い、廃棄処分とした。
最後に、Zenbook3。特に暖かいところに置いたわけではないが、キーボード部分が少し膨らんでおり、しっかり閉まらなくなってしまった。さすがに10万円以上して、まだまだ使えるZenbook3は修理に出す予定。今、ASUSに問い合わせ中。

だけど、MacBookは随分使ってるけど、バッテリーが膨らむようなことはないな〜。

Kali LinuxをノートPCに入れた場合に20分でサスペンドしてしまうのを回避する方法

/usr/share/gdm/dconf/99-local-settingsファイルを作成し、以下のように記述する。

[org/gnome/settings-daemon/plugins/power] 
sleep-inactive-ac-timeout=0
sleep-inactive-battery-timeout=0

以下のコマンドを実行

sudo dconf compile /var/lib/gdm3/greeter-dconf-defaults /usr/share/gdm/dconf

KAGOYA VPSでxrdpを設定

KAGOYAのVPSでxrdpを設定するまでの情報。
まず、Open VZ Ubuntu 14.04 64bitのインスタンスを作成する。
鍵ファイルを使いrootでsshログイン。
まずユーザーを追加。sudoに追加。

adduser hoge
gpasswd -a hoge sudo
apt-get update
apt-get upgrade -y
apt-get install sudo

ユーザーを追加できたら、そのユーザーでsshログインする。

sudo apt-get install build-essential devscripts git autoconf libtool pkg-config gcc g++ make libssl-dev libpam0g-dev libjpeg-dev libx11-dev libxfixes-dev libxrandr-dev flex bison libxml2-dev intltool xsltproc xutils-dev python-libxml2 g++ xutils libfuse-dev libmp3lame-dev nasm libpixman-1-dev xserver-xorg-dev -y

最新のバージョンを確認。https://launchpad.net/ubuntu/+source/xrdp
ソースからインストール。

cd ~
wget https://launchpad.net/ubuntu/+archive/primary/+files/xrdp_0.9.9.orig.tar.gz
wget https://launchpad.net/ubuntu/+archive/primary/+files/xrdp_0.9.5.orig-xorgxrdp.tar.gz
tar -zxvf xrdp_0.9.9.orig.tar.gz
tar -zxvf xrdp_0.9.5.orig-xorgxrdp.tar.gz
cd ~
cd xrdp-0.9.9
./bootstrap
./configure --enable-fuse
make
sudo make install
cd ~
cd xorgxrdp-0.2.5
./bootstrap
./configure
make
sudo make install

ポートを開ける。

sudo apt-get install iptables-persistent -y
sudo iptables -A INPUT -p tcp --dport 3389 -j ACCEPT
sudo iptables -A OUTPUT -p tcp --sport 3389 -j ACCEPT
sudo iptables -A INPUT -p udp --dport 3350 -j ACCEPT
sudo iptables -A OUTPUT -p udp --sport 3350 -j ACCEPT
sudo /etc/init.d/iptables-persistent save

その他、必要な設定など。

sudo apt-get -y install ubuntu-desktop
sudo apt-get install xfce4 xfce4-goodies -y
cd ~
echo xfce4-session > ~/.xsession
sudo reboot

サービスを立ち上げる。

sudo service xrdp start