hiroya taura
MPU6050のサンプルプログラム２
Dependencies: ConfigFile SDFileSystem mbed
Fork of
LAURUS_program
by 
LAURUS
Diff: main.cpp
- Revision:
- 15:d14d385d37e2
- Parent:
- 14:f85cb5340cb8
- Child:
- 16:174daf81eea0
- Child:
- 17:03b45055ca05
--- a/main.cpp Fri Jun 19 15:56:52 2015 +0000
+++ b/main.cpp Sat Jun 20 16:39:11 2015 +0000
@@ -16,6 +16,10 @@
#define TRUE 1
#define FALSE 0
+#define RULE1
+//#define RULE2
+//#define RULE3
+
const float dt = 0.01f; // 割り込み周期(s)
const float ServoMax = 0.0023f; // サーボの最大パルス長(s)
const float ServoMin = 0.0006f; // サーボの最小パルス長(s)
@@ -26,7 +30,9 @@
const int BorderParafoil = 0; // 物理スイッチのOFF出力
const int MaxCount = 3; // 投下シグナルを何回連続で検知したら投下と判断するか（×0.2[s]）
const int WaitTime = 1; // 投下後、安定するまで何秒滑空するか
-const float Alpha = 30.0f; // 目標方向と自分の進行方向との差の閾値（制御則1の定数
+const float Alpha = 30.0f; // 目標方向と自分の進行方向との差の閾値(deg)（制御則1&2&3の定数
+const float Beta = 60.0f; // 目標方向と自分の進行方向との間に取るべき角度差(deg)（制御則3の定数
+const float BorderDistance = 10.0f; // 落下制御に入るための目標値との距離の閾値(m)（制御則2の定数
/****************************** private macro ******************************/
/****************************** private typedef ******************************/
@@ -92,7 +98,7 @@
char data[512] = {}; // 送信データ用配列
/** config.txt **
-* #から始めるのはコメント行
+* #から始めるのはコメント行
* #イコールの前後に空白を入れない
* target_x=111.222
* target_y=33.444
@@ -137,27 +143,29 @@
void toString(Matrix& m); // デバッグ用
void toString(Vector& v); // デバッグ用
-inline float min(float a, float b) {
+inline float min(float a, float b)
+{
return ((a > b) ? b : a);
}
/****************************** main function ******************************/
-int main() {
-
+int main()
+{
+
i2c.frequency(400000); // I2Cの通信速度を400kHzに設定
-
+
if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!"); // mpu6050初期化
if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!"); // hmc5883l初期化
-
+
//Config読み取り
LoadConfig();
-
+
//SDカード初期化
FILE *fp;
char filename[15];
int n = Find_last();
- if(n < 0){
+ if(n < 0) {
pc.printf("Could not read a SD Card.\n");
return 0;
}
@@ -165,20 +173,20 @@
fp = fopen(filename, "w");
fprintf(fp, "log data\r\n");
xbee.printf("log data\r\n");
-
+
servoL.period(0.020f); // サーボの信号の周期は20ms
servoR.period(0.020f);
-
+
//カルマンフィルタ初期化
KalmanInit();
-
+
INT_timer.attach(&INT_func, dt); // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
-
+
//重力ベクトルの初期化
raw_g.SetComp(1, 0.0f);
raw_g.SetComp(2, 0.0f);
raw_g.SetComp(3, 1.0f);
-
+
// 機体に固定されたベクトルの初期化
b_f.SetComp(1, 0.0f);
b_f.SetComp(2, 0.0f);
@@ -187,97 +195,91 @@
b_u.SetComp(2, 0.0f);
b_u.SetComp(3, 0.0f);
b_l = Cross(b_u, b_f);
-
+
// 目標座標をベクトルに代入
target_p.SetComp(1, target_x * DEG_TO_RAD);
target_p.SetComp(2, target_y * DEG_TO_RAD);
-
+
/* ------------------------------ ↓↓↓ ここからメインループ ↓↓↓ ------------------------------ */
while(1) {
timer.stop();
timer.reset();
timer.start();
myled = 1; // LED is ON
-
-
+
+
/******************************* データ処理 *******************************/
{
INT_flag = FALSE; // 割り込みによる変数書き換えを阻止
gms.read(); // GPSデータ取得
UTC_t = (int)gms.time;
-
-
-
+
+
+
// 参照座標系の基底を求める
r_u = g;
r_f = geomag.GetPerpCompTo(g).Normalize();
r_l = Cross(r_u, r_f);
-
+
// 回転行列を経由してオイラー角を求める
// オイラー角はヨー・ピッチ・ロールの順に回転したものとする
// 各オイラー角を求めるのに回転行列の全成分は要らないので、一部だけ計算する。
-
+
float R_11 = r_f * b_f; // 回転行列の(1,1)成分
float R_12 = r_f * b_l; // 回転行列の(1,2)成分
float R_13 = r_f * b_u; // 回転行列の(1,3)成分
float R_23 = r_l * b_u; // 回転行列の(2,3)成分
float R_33 = r_u * b_u; // 回転行列の(3,3)成分
-
- yaw = atan2(-R_12, R_11) * RAD_TO_DEG - MAG_DECLINATION;
+
+#ifdef RULE3
+ yaw = atan2(-R_12, R_11) * RAD_TO_DEG + MAG_DECLINATION - Beta;
+#else
+ yaw = atan2(-R_12, R_11) * RAD_TO_DEG + MAG_DECLINATION;
+#endif
roll = atan2(-R_23, R_33) * RAD_TO_DEG;
pitch = asin(R_13) * RAD_TO_DEG;
-
+
if(yaw < -180.0f) yaw += 360.0f; // ヨー角を[-π, π]に補正
-
-
-
+ if(yaw > 180.0f) yaw -= 360.0f; // ヨー角を[-π, π]に補正
+
+
+
if(UTC_t - pre_UTC_t >= 1) { // GPSデータが更新されていたら
p.SetComp(1, gms.longitude * DEG_TO_RAD);
p.SetComp(2, gms.latitude * DEG_TO_RAD);
-
+
} else { // 更新されていなかったら
// GPSの補完処理を追加予定
}
-
+
pre_p = p;
pre_UTC_t = UTC_t;
-
+
float sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // サーボ電源電圧
float lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // ロジック電源電圧
-
+
// データをmicroSDに保存し、XBeeでPCへ送信する
- sprintf(data, "%.3f,%.3f,%.3f, %.3f,%.3f,%.3f, %.3f,%.3f,%.3f, %.3f,%d\r\n",
- yaw, pitch, roll,
- press, gms.longitude, gms.latitude,
- sv, lv, vrt_acc,
+ sprintf(data, "%.3f,%.3f,%.3f, %.3f,%.3f,%.3f, %.3f,%.3f,%.3f, %.3f,%d\r\n",
+ yaw, pitch, roll,
+ press, gms.longitude, gms.latitude,
+ sv, lv, vrt_acc,
Distance(target_p, p), optSensor.read_u16());
fprintf(fp, data);
fflush(fp);
xbee.printf(data);
-
+
INT_flag = TRUE; // 割り込み許可
}
-
-
+
+
/******************************* 制御ルーチン *******************************/
{
-
- if(fabs(roll) > BorderSpiral) {
- // スパイラル回避行動
- if(roll > 0) {
- pull_L = PullMax;
- pull_R = 0;
- } else {
- pull_L = 0;
- pull_R = PullMax;
- }
- } else {
-
- switch(step){
-
- // 投下検出
- case '0':
- if(thrown() || count != 0){
+
+ switch(step) {
+
+ // 投下&安定滑空シーケンス
+ case 0:
+ if(thrown() || count != 0) {
count++;
// 投下直後に紐を引く場合はコメントアウトをはずす
// pull_L = 15;
@@ -288,114 +290,141 @@
step = 1;
}
}
- break;
-
- // 制御ルーチン
- case '1':
- /* いずれも地球を完全球体と仮定 */
- float target_lng = target_x * DEG_TO_RAD;
- float target_lat = target_y * DEG_TO_RAD;
- /* 北から西回りで目標方向の角度を出力 */
- float targetY = cos( target_lat ) * sin( target_lng - p.GetComp(1) );
- float targetX = cos( p.GetComp(2) ) * sin( target_lat ) - sin( p.GetComp(2) ) * cos( target_lat ) * cos( target_lng - p.GetComp(1) );
- float theta = -atan2f( targetY, targetX );
- float delta_theta = 0.0f;
-
- if(yaw >= 0.0f) {
- if(theta >= 0.0f) {
- if(theta - yaw > Alpha) dir = 0;
- else if(theta - yaw < -Alpha) dir = 1;
+ break;
+
+ // 制御シーケンス
+ case 1:
+ if(fabs(roll) > BorderSpiral) {
+ // スパイラル回避行動
+ if(roll > 0) {
+ pull_L = PullMax;
+ pull_R = 0;
} else {
- theta += 360.0f;
- delta_theta = fabs(theta - yaw);
- if(delta_theta < 180.0f) {
- if(delta_theta > Alpha) dir = 0;
- } else {
- if(360.0f - delta_theta > Alpha) dir = 1;
- }
+ pull_L = 0;
+ pull_R = PullMax;
}
} else {
- if(theta <= 0.0f) {
- if(theta - yaw > Alpha) dir = 0;
- else if(theta - yaw < -Alpha) dir = 1;
- } else {
- theta -= 360.0f;
- delta_theta = fabs(theta - yaw);
- if(delta_theta < 180.0f) {
- if(delta_theta > Alpha) dir = 1;
- } else {
- if(360.0f - delta_theta > Alpha) dir = 0;
+
+ /* いずれも地球を完全球体と仮定 */
+ float target_lng = target_x * DEG_TO_RAD;
+ float target_lat = target_y * DEG_TO_RAD;
+ /* 北から西回りで目標方向の角度を出力 */
+ float targetY = cos( target_lat ) * sin( target_lng - p.GetComp(1) );
+ float targetX = cos( p.GetComp(2) ) * sin( target_lat ) - sin( p.GetComp(2) ) * cos( target_lat ) * cos( target_lng - p.GetComp(1) );
+ float theta = -atan2f( targetY, targetX );
+ float delta_theta = 0.0f;
+
+ if(yaw >= 0.0f) { // ヨー角が正
+ if(theta >= 0.0f) { // 目標角も正ならば、
+ if(theta - yaw > Alpha) dir = 0; // 単純に差を取って閾値αと比べる
+ else if(theta - yaw < -Alpha) dir = 1;
+ } else { // 目標角が負であれば
+ theta += 360.0f; // 360°足して正の値に変換してから
+ delta_theta = theta - yaw; // 差を取る（yawから左回りに見たthetaとの差分）
+ if(delta_theta < 180.0f) { // 差が180°より小さければ左旋回
+ if(delta_theta > Alpha) dir = 0;
+ } else { // 180°より大きければ右旋回
+ if(360.0f - delta_theta > Alpha) dir = 1;
+ }
}
+ } else { // ヨー角が負
+ if(theta <= 0.0f) { // 目標角も負ならば、
+ if(theta - yaw > Alpha) dir = 0; // 単純に差を取って閾値αと比べる
+ else if(theta - yaw < -Alpha) dir = 1;
+ } else { // 目標角が正であれば、
+ delta_theta = theta - yaw; // 差を取る（yawから左回りに見たthetaとの差分）
+ if(delta_theta < 180.0f) { // 差が180°より小さければ左旋回
+ if(delta_theta > Alpha) dir = 0;
+ } else { // 180°より大きければ右旋回
+ if(360.0f - delta_theta > Alpha) dir = 1;
+ }
+ }
+ }
+
+ if(dir == 0) { //目標は左方向
+
+ pull_L = 20;
+ pull_R = 0;
+
+ } else if (dir == 1) { //目標は右方向
+
+ pull_L = 0;
+ pull_R = 20;
+
}
}
-
-
- if(dir == 0) { //目標は左方向
-
- pull_L = 20;
- pull_R = 0;
-
- } else if (dir == 1) { //目標は右方向
-
- pull_L = 0;
- pull_R = 20;
-
- }
-
- break;
- }
-
+
+#ifdef RULE2
+ // 目標地点との距離が閾値以下だった場合、落下シーケンスへと移行する
+ if(Distance(target_p, p) < BorderDistance) step = 2;
+#endif
+
+ break;
+
+#ifdef RULE2
+ // 落下シーケンス
+ case 2:
+ pull_L = 25;
+ pull_R = 0;
+
+ // もし落下中に目標値から離れてしまった場合は、体勢を立て直して再び滑空
+ // 境界で制御が不安定にならないよう閾値にマージンを取る
+ if(Distance(target_p, p) > BorderDistance+5.0f) step = 1;
+ break;
+#endif
}
-
+
// 指定された引っ張り量だけ紐を引っ張る
if(pull_L < 0) pull_L = 0;
else if(pull_L > PullMax) pull_L = PullMax;
if(pull_R < 0) pull_R = 0;
else if(pull_R > PullMax) pull_R = PullMax;
-
+
servoL.pulsewidth((ServoMax - ServoMin) * pull_L / PullMax + ServoMin);
servoR.pulsewidth((ServoMax - ServoMin) * pull_R / PullMax + ServoMin);
-
-
-
+
+
+
}
-
+
myled = 0; // LED is OFF
-
+
// ループはきっかり0.2秒ごと
while(timer.read_ms() < 200);
-
-
+
+
}
-
+
/* ------------------------------ ↑↑↑ ここまでメインループ ↑↑↑ ------------------------------ */
}
-void LoadConfig(){
+void LoadConfig()
+{
char value[20];
//Read a configuration file from a mbed.
- if (!cfg.read("/sd/config.txt")){
+ if (!cfg.read("/sd/config.txt")) {
pc.printf("Config file does not exist\n");
- }else{
+ } else {
//Get values
if (cfg.getValue("target_x", &value[0], sizeof(value))) target_x = atof(value);
- else{
+ else {
pc.printf("Failed to get value for target_x\n");
}
if (cfg.getValue("target_y", &value[0], sizeof(value))) target_y = atof(value);
- else{
+ else {
pc.printf("Failed to get value for target_y\n");
}
}
}
-int Find_last() {
+int Find_last()
+{
int i, n = 0;
char c;
DIR *dp;
struct dirent *dirst;
dp = opendir("/sd/");
- if (!dp){
+ if (!dp) {
pc.printf("Could not open directry.\n");
return -1;
}
@@ -408,7 +437,8 @@
return n;
}
-void KalmanInit() {
+void KalmanInit()
+{
// 重力
{
// 誤差共分散行列の値を決める（対角成分のみ）
@@ -416,17 +446,17 @@
float alpha_Q2 = 0.5f;
R2 *= alpha_R2;
Q2 *= alpha_Q2;
-
+
// 観測方程式のヤコビアンの値を設定（時間変化無し）
float h2[15] = {
1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
- 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
+ 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f, 0.0f
};
-
+
H2.SetComps(h2);
}
-
+
// 地磁気
{
// 誤差共分散行列の値を決める（対角成分のみ）
@@ -434,49 +464,50 @@
float alpha_Q1 = 0.5f;
R1 *= alpha_R1;
Q1 *= alpha_Q1;
-
+
// 観測方程式のヤコビアンの値を設定（時間変化無し）
float h1[21] = {
1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f
};
-
+
H1.SetComps(h1);
}
}
-void KalmanUpdate() {
+void KalmanUpdate()
+{
// 重力
-
+
{
// ヤコビアンの更新
float f2[25] = {
- 1.0f, (raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, -(raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, 0.0f, post_x2.GetComp(3)*dt,
- -(raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, 1.0f, (raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, -post_x2.GetComp(3)*dt, 0.0f,
- (raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, -(raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 1.0f, post_x2.GetComp(2)*dt, -post_x2.GetComp(1)*dt,
- 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
+ 1.0f, (raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, -(raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, 0.0f, post_x2.GetComp(3)*dt,
+ -(raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, 1.0f, (raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, -post_x2.GetComp(3)*dt, 0.0f,
+ (raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, -(raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 1.0f, post_x2.GetComp(2)*dt, -post_x2.GetComp(1)*dt,
+ 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
0.0f, 0.0f, 0.0f, 0.0f, 1.0f
};
-
+
F2.SetComps(f2);
-
+
// 事前推定値の更新
//pri_x2 = F2 * post_x2;
-
+
float pri_x2_vals[5] = {
- post_x2.GetComp(1) + post_x2.GetComp(2) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt - post_x2.GetComp(3) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt,
- post_x2.GetComp(2) + post_x2.GetComp(3) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt - post_x2.GetComp(1) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt,
- post_x2.GetComp(3) + post_x2.GetComp(1) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt - post_x2.GetComp(2) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt,
- post_x2.GetComp(4),
+ post_x2.GetComp(1) + post_x2.GetComp(2) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt - post_x2.GetComp(3) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt,
+ post_x2.GetComp(2) + post_x2.GetComp(3) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt - post_x2.GetComp(1) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt,
+ post_x2.GetComp(3) + post_x2.GetComp(1) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt - post_x2.GetComp(2) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt,
+ post_x2.GetComp(4),
post_x2.GetComp(5)
};
-
+
pri_x2.SetComps(pri_x2_vals);
-
+
// 事前誤差分散行列の更新
pri_P2 = F2 * post_P2 * F2.Transpose() + R2;
-
+
// カルマンゲインの計算
S2 = Q2 + H2 * pri_P2 * H2.Transpose();
static float det;
@@ -484,47 +515,47 @@
pc.printf("E:%.3f\r\n", det);
return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
}
- K2 = pri_P2 * H2.Transpose() * S_inv2;
-
+ K2 = pri_P2 * H2.Transpose() * S_inv2;
+
// 事後推定値の更新
post_x2 = pri_x2 + K2 * (raw_acc - H2 * pri_x2);
// 事後誤差分散行列の更新
post_P2 = (I2 - K2 * H2) * pri_P2;
}
-
-
+
+
// 地磁気
{
// ヤコビアンの更新
float f1[49] = {
- 1.0f, (raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, -(raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, 0.0f, 0.0f, 0.0f, -post_x1.GetComp(2) * dt,
- -(raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, 1.0f, (raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 0.0f, 0.0f, 0.0f, post_x1.GetComp(1) * dt,
- (raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, -(raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
- 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
- 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
- 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
+ 1.0f, (raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, -(raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, 0.0f, 0.0f, 0.0f, -post_x1.GetComp(2) * dt,
+ -(raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, 1.0f, (raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 0.0f, 0.0f, 0.0f, post_x1.GetComp(1) * dt,
+ (raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, -(raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
};
-
+
F1.SetComps(f1);
-
+
// 事前推定値の更新
//pri_x1 = F1 * post_x1;
float pri_x1_vals[7] = {
- post_x1.GetComp(1) + post_x1.GetComp(2) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt - post_x1.GetComp(3) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt,
- post_x1.GetComp(2) + post_x1.GetComp(3) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt - post_x1.GetComp(1) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt,
- post_x1.GetComp(3) + post_x1.GetComp(1) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt - post_x1.GetComp(2) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt,
- post_x1.GetComp(4),
- post_x1.GetComp(5),
- post_x1.GetComp(6),
+ post_x1.GetComp(1) + post_x1.GetComp(2) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt - post_x1.GetComp(3) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt,
+ post_x1.GetComp(2) + post_x1.GetComp(3) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt - post_x1.GetComp(1) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt,
+ post_x1.GetComp(3) + post_x1.GetComp(1) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt - post_x1.GetComp(2) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt,
+ post_x1.GetComp(4),
+ post_x1.GetComp(5),
+ post_x1.GetComp(6),
post_x1.GetComp(7)
};
-
+
pri_x1.SetComps(pri_x1_vals);
-
+
// 事前誤差分散行列の更新
pri_P1 = F1 * post_P1 * F1.Transpose() + R1;
-
+
// カルマンゲインの計算
S1 = Q1 + H1 * pri_P1 * H1.Transpose();
static float det;
@@ -532,8 +563,8 @@
pc.printf("E:%.3f\r\n", det);
return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
}
- K1 = pri_P1 * H1.Transpose() * S_inv1;
-
+ K1 = pri_P1 * H1.Transpose() * S_inv1;
+
// 事後推定値の更新
post_x1 = pri_x1 + K1 * (raw_geomag - H1 * pri_x1);
// 地磁気ベクトルの大きさに拘束条件を与える
@@ -542,9 +573,9 @@
gm.SetComp(1, post_x1.GetComp(1));
gm.SetComp(2, post_x1.GetComp(2));
gm.SetComp(3, post_x1.GetComp(3));
-
+
gm = MAG_MAGNITUDE * gm.Normalize();
-
+
post_x1.SetComp(1, gm.GetComp(1));
post_x1.SetComp(2, gm.GetComp(2));
post_x1.SetComp(3, gm.GetComp(3));
@@ -554,9 +585,10 @@
}
}
-float Distance(Vector p1, Vector p2) {
+float Distance(Vector p1, Vector p2)
+{
if(p1.GetDim() != p2.GetDim()) return 0.0f;
-
+
static float mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f;
static float s_mu_y = sin(mu_y);
static float w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y);
@@ -564,60 +596,62 @@
static float n = GPS_A / w;
static float d1 = m * (p1.GetComp(2) - p2.GetComp(2));
static float d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1));
-
+
return sqrt(d1 * d1 + d2 * d2);
}
/** 投下を検知する関数
- *
+ *
* @return 投下されたかどうか（true=投下 false=未投下
- *
+ *
*/
-bool thrown() {
+bool thrown()
+{
static int opt_count = 0;
static int g_count = 0;
static int para_count = 0;
-
+
if(optSensor.read_u16() > BorderOpt) opt_count++;
else opt_count = 0;
if(vrt_acc < BorderGravity) g_count++;
else g_count = 0;
if((int)paraSensor == BorderParafoil) para_count++;
else para_count = 0;
-
+
if(opt_count >= MaxCount || g_count >= MaxCount || para_count >= MaxCount) {
return true;
}
-
+
return false;
-
+
}
/* ------------------------------ 割り込み関数 ------------------------------ */
-void INT_func() {
+void INT_func()
+{
// センサーの値を更新
mpu.read();
hmc.read();
-
+
for(int i=0; i<3; i++) {
raw_acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G);
raw_gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
raw_geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS);
}
-
+
Vector delta_g = Cross(raw_gyro, raw_g); // Δg = ω × g
raw_g = 0.9f * (raw_g - delta_g * dt) + 0.1f * raw_acc.Normalize(); // 相補フィルタ
raw_g = raw_g.Normalize();
-
+
KalmanUpdate();
-
+
// LPS25Hによる気圧の取得は10Hz
lps_cnt = (lps_cnt+1)%10;
if(lps_cnt == 0) {
raw_press = (float)lps.pressure() * PRES_LSB_TO_HPA;
}
-
+
if(INT_flag != FALSE) {
g = raw_g;
for(int i=0; i<3; i++) {
@@ -626,30 +660,32 @@
acc = raw_acc;
gyro = raw_gyro;
press = raw_press;
-
+
vrt_acc = raw_acc * raw_g;
-
+
}
}
/* ------------------------------ デバッグ用関数 ------------------------------ */
-void toString(Matrix& m) {
-
+void toString(Matrix& m)
+{
+
for(int i=0; i<m.GetRow(); i++) {
for(int j=0; j<m.GetCol(); j++) {
pc.printf("%.6f\t", m.GetComp(i+1, j+1));
}
pc.printf("\r\n");
}
-
+
}
-void toString(Vector& v) {
-
+void toString(Vector& v)
+{
+
for(int i=0; i<v.GetDim(); i++) {
pc.printf("%.6f\t", v.GetComp(i+1));
}
pc.printf("\r\n");
-
+
}
\ No newline at end of file