CIA
エラー判定用 '\0' + クォータニオンのfloat型4成分，合計17byteのデータをひたすらバイナリ形式で送り続ける．最初のバイトが '\0' でなかったらズレているので，適当なバイト数見送って先頭が '\0' になるよう合わせること．
main.cpp
- Committer:
- daqn
- Date:
- 2018-03-21
- Revision:
- 0:2a7221ee9861
File content as of revision 0:2a7221ee9861:
#include "mbed.h"
#include "MPU9250.h"
#include "math.h"
#include "Eigen/Core.h" // 線形代数のライブラリ
#include "Eigen/Geometry.h" // 同上
#define PI 3.141592654 // 円周率
Timer timer;
Ticker ticker;
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
//InterruptIn flt_pin(p30);
Serial pc (USBTX, USBRX);
MPU9250 mpu(p28,p27);
const float t_calibrate = 25.f;
const float dt = 0.05;
// 変数用意
int N = 0; // 較正に用いるサンプル数
float gyro[3]; // ジャイロセンサの出力
float b_gyro[3] = {}; // ジャイロセンサのバイアス誤差．
float acc[3]; // 加速度センサの出力
// float acc_norm; // 加速度ベクトルの2-ノルム（長さ）．
float acc_normalized[3]; // 正規化した（長さ1の）加速度ベクトル．
float pitch = 0.f; // ピッチ角.
float roll = 0.f; // バンク（ロール）角.
float yaw; // 方位角（ヨー角）．
Eigen::Vector4f q; // 姿勢クォータニオン
Eigen::Matrix4f Omg;
Eigen::Vector4f q_dot;
float omg[3];
float q00_22, q11_33;
void calibrate();
void set_initial_quaternion(float roll, float pitch);
void flt_pin_rise();
void flt_pin_fall();
void update();
void update_correction();
int main()
{
mpu.setAcceleroRange(MPU9250_ACCELERO_RANGE_16G);
mpu.setGyroRange(MPU9250_GYRO_RANGE_2000);
led1 = 1;
led2 = 1;
led3 = 1;
// pc.printf("=\r\n\n");
// pc.printf("Calibration (%3.1f s) will start in\r\n 5\r\n", t_calibrate);
wait(1.0);
// pc.printf(" 4\r\n");
wait(1.0);
// pc.printf(" 3\r\n");
wait(1.0);
// pc.printf(" 2\r\n");
wait(1.0);
// pc.printf(" 1\r\n\n");
wait(1.0);
led1 = 0;
// pc.printf("Calibrating... ");
ticker.attach(&calibrate, 0.01);
timer.start();
while (timer.read() < t_calibrate);
ticker.detach();
// pc.printf("Done. (%d samples)\r\n\n", N);
// timer.stop();
led2 = 0;
b_gyro[0] /= N;
b_gyro[1] /= N;
b_gyro[2] /= N;
roll /= N;
pitch /= N;
// printf("Initial Attitude\r\n");
// printf(" bank:%6.1f deg, pitch:%6.1f deg\r\n\n",
// roll*180/PI , pitch*180/PI);
set_initial_quaternion(roll, pitch);
led3 = 0;
// pc.printf("waiting for launch...\r\n");
// flt_pin.mode(PullUp);
// flt_pin.fall(&flt_pin_fall);
// flt_pin.rise(&flt_pin_rise);
ticker.attach(&update_correction, dt);
led4 = 1;
while(1);
}
void calibrate()
{
//////////////////////////////////////
// //
// ジャイロセンサの較正・初期姿勢角の取得 //
// //
//////////////////////////////////////
// b_gyro にはジャイロセンサの出力の平均（バイアス誤差）を格納．
mpu.getGyro(gyro);
b_gyro[0] += gyro[0];
b_gyro[1] += gyro[1];
b_gyro[2] += gyro[2];
// 重力加速度ベクトルを使って，姿勢角を算出
mpu.getAccelero(acc);
// acc_norm = sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
// acc_normalized[0] = acc[0] / acc_norm;
// acc_normalized[1] = acc[1] / acc_norm;
// acc_normalized[2] = acc[2] / acc_norm;
acc_normalized[2] = acc[2] / sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
// N 回の姿勢角の平均をとって初期姿勢角とする
pitch += asin (acc_normalized[2]);
roll += atan2(acc[1], -acc[0]);
// count up
N++;
}
void set_initial_quaternion(float roll, float pitch)
{
/////////////////////////////////////////
// //
// 初期姿勢角から初期姿勢クォータニオンを計算 //
// //
/////////////////////////////////////////
float c_roll2 = cos(roll/2); // 繰り返しの計算を避けるための一時変数
float s_roll2 = sin(roll/2); // 同上
Eigen::Matrix4f qx0_otimes;
qx0_otimes << c_roll2,-s_roll2, 0 , 0 ,
s_roll2, c_roll2, 0 , 0 ,
0 , 0 , c_roll2, s_roll2,
0 , 0 ,-s_roll2, c_roll2;
Eigen::Vector4f q_y0(cos(pitch/2), 0.f, sin(pitch/2), 0.f);
// 初期姿勢クォータニオン q
q = qx0_otimes * q_y0;
}
void flt_pin_rise()
{
pc.printf("Launch!\r\n");
ticker.attach(&update, dt);
}
void flt_pin_fall()
{
pc.printf("fall.\r\n");
ticker.detach();
}
/**
* 観測値を用いて姿勢角を更新します．
* ジャイロセンサの出力を単純に積算します．
*/
void update()
{
mpu.getGyro(gyro);
omg[0] = gyro[2] - b_gyro[2];
omg[1] = -gyro[1] + b_gyro[1];
omg[2] = gyro[0] - b_gyro[0];
// 姿勢クォータニオンの時間微分を計算
Omg << 0.f ,-omg[0],-omg[1],-omg[2],
omg[0], 0.f , omg[2],-omg[1],
omg[1],-omg[2], 0.f , omg[0],
omg[2], omg[1],-omg[0], 0.f ;
// q_dot = 0.5f * Omg * q;
// 現在時刻の姿勢クォータニオン
q += Omg * q * 0.5f * dt;
q /= sqrtf(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
// 姿勢クォータニオンからオイラー角への変換
q00_22 = (q[0] + q[2])*(q[0] - q[2]);
q11_33 = (q[1] + q[3])*(q[1] - q[3]);
roll = atan2( 2*(q[2]*q[3]+q[0]*q[1]), q00_22 - q11_33);
pitch = asin (-2*(q[1]*q[3]-q[0]*q[2]));
// yaw = atan2( 2*(q[1]*q[2]+q[0]*q[3]), q00_22 + q11_33);
// pc.printf("%11.7f ",timer.read());
// pc.printf("%6.1f %6.1f %6.1f\r\n",
// roll*180/PI, pitch*180/PI, yaw*180/PI);
// pc.printf("%6.1f %6.1f\r\n",
// roll*180/PI, pitch*180/PI);
}
Eigen::Vector3f bB_tilde;
Eigen::Vector3f bB_hat;
Eigen::Vector3f bG(0.f, 0.f, -1.f);
Eigen::Vector3f rot_vec;
float rot_q[4] = {1.f, 0.f, 0.f, 0.f};
Eigen::Matrix3f dcm;
Eigen::Matrix4f rot_q_otimes;
const float eps = 0.01;
float q00, q01, q02, q03, q11, q12, q13, q22, q23, q33;
float a_norm_div;
float send_buff[4];
char* send_bufc = (char*)send_buff;
/**
* 観測値を用いて姿勢角を更新します．
* ジャイロセンサの出力を積算した上で，加速度センサの出力を用いて補正します．
*/
void update_correction()
{
mpu.getGyro(gyro);
omg[0] = gyro[2] - b_gyro[2];
omg[1] = -gyro[1] + b_gyro[1];
omg[2] = gyro[0] - b_gyro[0];
// 姿勢クォータニオンの時間微分を計算
Omg << 0.f ,-omg[0],-omg[1],-omg[2],
omg[0], 0.f , omg[2],-omg[1],
omg[1],-omg[2], 0.f , omg[0],
omg[2], omg[1],-omg[0], 0.f ;
// q_dot = 0.5f * Omg * q;
// 現在時刻の姿勢クォータニオン
q += Omg * q * 0.5f * dt;
q /= sqrtf(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
// 姿勢クォータニオンからオイラー角への変換
q00 = q[0]*q[0];
q01 = q[0]*q[1];
q02 = q[0]*q[2];
q03 = q[0]*q[3];
q11 = q[1]*q[1];
q12 = q[1]*q[2];
q13 = q[1]*q[3];
q22 = q[2]*q[2];
q23 = q[2]*q[3];
q33 = q[3]*q[3];
dcm << q00+q11-q22-q33, 2*(q12+q03) , 2*(q13-q02) ,
2*(q12-q03) , q00-q11+q22-q33, 2*(q23+q01) ,
2*(q13+q02) , 2*(q23-q01) , q00-q11-q22+q33;
bB_hat = dcm*bG;
mpu.getAccelero(acc);
a_norm_div = 1.f / sqrt(acc[0]*acc[0] + acc[1]*acc[1] + acc[2]*acc[2]);
bB_tilde << acc[2]*a_norm_div,-acc[1]*a_norm_div, acc[0]*a_norm_div;
rot_vec = bB_hat.cross(bB_hat - bB_tilde);
rot_q[1] = 0.5f*eps*rot_vec[0];
rot_q[2] = 0.5f*eps*rot_vec[1];
rot_q[3] = 0.5f*eps*rot_vec[2];
rot_q_otimes << 1.f ,-rot_q[1],-rot_q[2],-rot_q[3],
rot_q[1], 1.f , rot_q[3],-rot_q[2],
rot_q[2],-rot_q[3], 1.f , rot_q[1],
rot_q[3], rot_q[2],-rot_q[1], 1.f ;
q = rot_q_otimes * q;
// q00 = q[0]*q[0]; // Note that these values are based on
// q11 = q[1]*q[1]; // rotated quatenion (L253), so they are diferent from
// q22 = q[2]*q[2]; // the values calculated above (L228-237).
// q33 = q[3]*q[3];
// q /= sqrtf(q00 + q11 + q22 + q33);
q /= sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
send_buff[0] = q[0];
send_buff[1] = q[1];
send_buff[2] = q[2];
send_buff[3] = q[3];
// send_buff[0] = 0.1f;
// send_buff[1] = 0.2f;
// send_buff[2] = 0.3f;
// send_buff[3] = 0.4f;
// pc.write(send_buf,4);
pc.putc('\0');
for (int i = 0; i < 16; i++)
pc.putc(send_bufc[i]);
// q00_22 = q00 - q22;
// q11_33 = q11 - q33;
// roll = atan2( 2*(q[2]*q[3]+q[0]*q[1]), q00_22 - q11_33);
// pitch = asin (-2*(q[1]*q[3]-q[0]*q[2]));
// yaw = atan2( 2*(q[1]*q[2]+q[0]*q[3]), q00_22 + q11_33);
// pc.printf("%11.7f ",timer.read());
// pc.printf("%6.1f %6.1f %6.1f\r\n",
// roll*180/PI, pitch*180/PI, yaw*180/PI);
// pc.printf("%6.1f %6.1f\r\n",
// roll*180/PI, pitch*180/PI);
}