LAURUS
output only raw data (acceleration, anguler rate, geomagnetism, air pressure)
Dependencies: mbed SDFileSystem ConfigFile
Revision 19:ad8ff2de68f5, committed 2015-06-22
- Comitter:
- ojan
- Date:
- Mon Jun 22 15:23:37 2015 +0000
- Parent:
- 18:9dd72e417c60
- Parent:
- 17:03b45055ca05
- Child:
- 20:00afba164688
- Commit message:
- LAURUS_Program_v.2.1; ; + some refactoring; + GPS bug fixed (by Onaka
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp.orig | Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Jun 22 15:16:00 2015 +0000
+++ b/main.cpp Mon Jun 22 15:23:37 2015 +0000
@@ -46,6 +46,7 @@
Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート
GMS6_CR6 gms(&gps, &pc); // GPS
SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); // microSD
+FILE * fp; // ログファイルのポインタ
BufferedSerial xbee(PA_9, PA_10, PC_1); // Xbee
ConfigFile cfg; // ConfigFile
PwmOut servoL(PB_6), servoR(PC_7); // サーボ用PWM出力
@@ -58,10 +59,12 @@
int lps_cnt = 0; // 気圧センサ読み取りカウント
uint8_t INT_flag = TRUE; // 割り込み可否フラグ
+/* こちらの変数群はメインループでは参照しない */
Vector raw_acc(3); // 加速度(m/s^2) 生
Vector raw_gyro(3); // 角速度(deg/s) 生
Vector raw_geomag(3); // 地磁気(?) 生
float raw_press; // 気圧(hPa) 生
+/* メインループ内ではこちらを参照する */
Vector acc(3); // 加速度(m/s^2)
Vector gyro(3); // 角速度(deg/s)
Vector geomag(3); // 地磁気(?)
@@ -133,9 +136,11 @@
/****************************** private functions ******************************/
+void SD_Init(); // SDカード初期化
+void VectorsInit(); // 各種ベクトルの初期化
+void KalmanInit(); // カルマンフィルタ初期化
void LoadConfig(); // config読み取り
int Find_last(); // SDカード初期化用関数
-void KalmanInit(); // カルマンフィルタ初期化
void KalmanUpdate(); // カルマンフィルタ更新
float Distance(Vector p1, Vector p2); // 緯度・経度の差から2点間の距離を算出(m)
bool thrown(); // 投下されたかどうかを判断する
@@ -161,44 +166,20 @@
//Config読み取り
LoadConfig();
- //SDカード初期化
- FILE *fp;
- char filename[15];
- int n = Find_last();
- if(n < 0) {
- pc.printf("Could not read a SD Card.\n");
- return 0;
- }
- sprintf(filename, "/sd/log%03d.csv", n+1);
- 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);
+ // SDカード初期化
+ SD_Init();
//カルマンフィルタ初期化
KalmanInit();
+ // 各種ベクトルの初期化
+ VectorsInit();
+
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);
+
+ servoL.period(0.020f); // サーボの信号の周期は20ms
+ servoR.period(0.020f);
- // 機体に固定されたベクトルの初期化
- b_f.SetComp(1, 0.0f);
- b_f.SetComp(2, 0.0f);
- b_f.SetComp(3, -1.0f);
- b_u.SetComp(1, 1.0f);
- 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) {
@@ -214,8 +195,6 @@
gms.read(); // GPSデータ取得
UTC_t = (int)gms.time;
-
-
// 参照座標系の基底を求める
r_u = g;
r_f = geomag.GetPerpCompTo(g).Normalize();
@@ -242,8 +221,6 @@
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);
@@ -259,20 +236,14 @@
float lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // ロジック電源電圧
// データをmicroSDに保存し、XBeeでPCへ送信する
- sprintf(data, "%d, %.1f,%.1f,%.1f, %.3f,%.5f,%.5f",
+ sprintf(data, "%d, %.1f,%.1f,%.1f, %.3f,%.5f,%.5f, %.3f,%.3f,%.3f, %.3f,%d\r\n",
UTC_t, yaw, pitch, roll,
- press, gms.longitude, gms.latitude);
+ press, gms.longitude, gms.latitude,
+ sv, lv, vrt_acc,
+ Distance(target_p, p), optSensor.read_u16());
fprintf(fp, data);
fflush(fp);
- xbee.printf(data);
-
- sprintf(data, ", %.3f,%.3f,%.3f, %.3f,%d\r\n",
- sv, lv, vrt_acc,
- Distance(target_p, p), optSensor.read_u16());
-
- fprintf(fp, data);
- fflush(fp);
- xbee.printf(data);
+ xbee.printf(data);
INT_flag = TRUE; // 割り込み許可
}
@@ -359,7 +330,7 @@
}
}
-
+
#ifdef RULE2
// 目標地点との距離が閾値以下だった場合、落下シーケンスへと移行する
if(Distance(target_p, p) < BorderDistance) step = 2;
@@ -370,7 +341,7 @@
#ifdef RULE2
// 落下シーケンス
case 2:
- pull_L = 25;
+ pull_L = PullMax;
pull_R = 0;
// もし落下中に目標値から離れてしまった場合は、体勢を立て直して再び滑空
@@ -389,21 +360,61 @@
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 VectorsInit() {
+ //重力ベクトルの初期化
+ 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);
+ b_f.SetComp(3, -1.0f);
+ b_u.SetComp(1, 1.0f);
+ 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);
+}
+
+/** マイクロSDカードの初期化を行う関数
+ *
+ */
+void SD_Init() {
+ //SDカード初期化
+ char filename[15];
+ int n = Find_last();
+ if(n < 0) {
+ pc.printf("Could not read a SD Card.\n");
+ return;
+ }
+ sprintf(filename, "/sd/log%03d.csv", n+1);
+ fp = fopen(filename, "w");
+ fprintf(fp, "log data\r\n");
+ xbee.printf("log data\r\n");
+}
+
+/** コンフィグファイルを読み込む関数
+ *
+ */
void LoadConfig()
{
char value[20];
@@ -423,6 +434,10 @@
}
}
+/** ログファイルの番号の最大値を得る関数
+ *
+ * @return マイクロSD内に存在するログファイル番号の最大値
+ */
int Find_last()
{
int i, n = 0;
@@ -443,6 +458,9 @@
return n;
}
+/** カルマンフィルタの初期化を行う関数
+ *
+ */
void KalmanInit()
{
// 重力
@@ -482,6 +500,9 @@
}
}
+/** カルマンフィルタの更新を行う関数
+ *
+ */
void KalmanUpdate()
{
// 重力
@@ -573,24 +594,17 @@
// 事後推定値の更新
post_x1 = pri_x1 + K1 * (raw_geomag - H1 * pri_x1);
- // 地磁気ベクトルの大きさに拘束条件を与える
- /*{
- Vector gm(3);
- 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));
- }*/
// 事後誤差分散行列の更新
post_P1 = (I1 - K1 * H1) * pri_P1;
}
}
+/** GPS座標から距離を算出
+ * @param 座標1（経度、緯度）（rad）
+ * @param 座標2（経度、緯度）（rad）
+ *
+ * @return 2点間の距離（m）
+ */
float Distance(Vector p1, Vector p2)
{
if(p1.GetDim() != p2.GetDim()) return 0.0f;
@@ -610,7 +624,7 @@
*
* @return 投下されたかどうか（true=投下 false=未投下
*
-*/
+ */
bool thrown()
{
static int opt_count = 0;
--- a/main.cpp.orig Mon Jun 22 15:16:00 2015 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,321 +0,0 @@
-#include "mbed.h"
-#include "MPU6050.h"
-#include "HMC5883L.h"
-#include "LPS25H.h"
-#include "GMS6_CR6.h"
-#include "ErrorLogger.h"
-#include "Vector.h"
-#include "Matrix.h"
-#include "Vector_Matrix_operator.h"
-#include "myConstants.h"
-#include "SDFileSystem.h"
-#include "BufferedSerial.h"
-#include "ConfigFile.h"
-
-/********** private define **********/
-#define TRUE 1
-#define FALSE 0
-/********** private macro **********/
-
-/********** private typedef **********/
-
-/********** private variables **********/
-DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力
-I2C i2c(PB_9, PB_8); // I2Cポート
-MPU6050 mpu(&i2c); // 加速度・角速度センサ
-HMC5883L hmc(&i2c); // 地磁気センサ
-LPS25H lps(&i2c); // 気圧センサ
-Serial gps(PA_11, PA_12); // GPS通信用シリアルポート
-Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート
-GMS6_CR6 gms(&gps, &pc); // GPS
-Ticker INT_timer; // 割り込みタイマー
-SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); // microSD
-BufferedSerial xbee(PA_9, PA_10, PC_1); // Xbee
-ConfigFile cfg; //ConfigFile
-PwmOut servoL(PC_7), servoR(PB_6);
-AnalogIn rf(PC_0);
-AnalogIn servoVcc(PA_0);
-AnalogIn logicVcc(PA_1);
-
-
-const float dt = 0.1f; // 割り込み周期（s）
-
-int lps_cnt = 0; // 気圧センサ読み取りカウント
-uint8_t INT_flag = TRUE; // 割り込み可否フラグ
-Vector raw_acc(3); // 加速度(m/s^2) 生
-Vector raw_gyro(3); // 角速度(deg/s) 生
-Vector raw_geomag(3); // 地磁気(?) 生
-float raw_press; // 気圧(hPa) 生
-Vector acc(3); // 加速度(m/s^2)
-Vector gyro(3); // 角速度(deg/s)
-Vector geomag(3); // 地磁気(?)
-float press; // 気圧(hPa)
-
-Vector raw_g(3); // 重力ベクトル 生
-Vector g(3); // 重力ベクトル 生
-//Vector n(3); // 地磁気ベクトル
-
-/** config.txt **
-* #から始めるのはコメント行
-* #イコールの前後に空白を入れない
-* target_x=111.222
-* target_y=33.444
-*/
-float target_x, target_y;
-
-/* ----- Kalman Filter ----- */
-Vector pri_x(6);
-Matrix pri_P(6, 6);
-Vector post_x(6);
-Matrix post_P(6, 6);
-Matrix F(6, 6), H(3, 6);
-Matrix R(6, 6), Q(3, 3);
-Matrix I(6, 6);
-Matrix K(6, 3);
-Matrix S(3, 3), inv(3, 3);
-/* ----- ------------- ----- */
-
-Timer timer;
-
-char data[512] = {};
-
-/********** private functions **********/
-void LoadConfig(); // config読み取り
-int find_last(); // SDカード初期化用関数
-void KalmanInit(); // カルマンフィルタ初期化
-void KalmanUpdate(); // カルマンフィルタ更新
-void INT_func(); // 割り込み用関数
-void toString(Matrix& m);
-void toString(Vector& v);
-
-/********** main function **********/
-
-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){
- pc.printf("Could not read a SD Card.\n");
- return 0;
- }
- sprintf(filename, "/sd/log%03d.csv", n+1);
- fp = fopen(filename, "w");
- fprintf(fp, "log data\r\n");
- xbee.printf("log data\r\n");
-
- //カルマンフィルタ初期化
- 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);
-
- /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */
- while(1) {
- timer.stop();
- timer.reset();
- timer.start();
- // 0.1秒おきにセンサーの出力をpcへ出力
- myled = 1; // LED is ON
- wait(0.05f); // 50 ms
- myled = 0; // LED is OFF
-
- INT_flag = FALSE; // 割り込みによる変数書き換えを阻止
-
- float sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
- float lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
-
- sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%d\r\n",
- g.GetComp(1), g.GetComp(2), g.GetComp(3),
- geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3),
- press, gms.longitude, gms.latitude,
- sv, lv, gms.Ns);
- fprintf(fp, data);
- xbee.printf(data);
-
- INT_flag = TRUE; // 割り込み許可
-
- pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f);
-
- // 制御ルーチン
- {
- }
-
-
- // ループはきっかり1秒ごと
- while(timer.read_ms() < 1000);
-
- }
-
- /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */
- //fclose(fp);
-}
-
-void LoadConfig(){
- char value[20];
- //Read a configuration file from a mbed.
- if (!cfg.read("/sd/config.txt")){
- pc.printf("Config file does not exist\n");
- }else{
- //Get values
- if (cfg.getValue("target_x", &value[0], sizeof(value))) target_x = atof(value);
- 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{
- pc.printf("Failed to get value for target_y\n");
- }
- }
-}
-
-int find_last() {
- int i, n = 0;
- char c;
- DIR *dp;
- struct dirent *dirst;
- dp = opendir("/sd/");
- if (!dp){
- pc.printf("Could not open directry.\n");
- return -1;
- }
- while((dirst = readdir(dp)) != NULL) {
- if(sscanf(dirst->d_name, "log%03d.csv%c", &i, &c) == 1 && i>n) {
- n = i;
- }
- }
- closedir(dp);
- return n;
-}
-
-void KalmanInit() {
-
- // 誤差共分散行列の値を決める（対角成分のみ）
- float alpha_R = 60.0f;
- float alpha_Q = 100.0f;
- R *= alpha_R;
- Q *= alpha_Q;
-
- // 状態方程式のヤコビアンの初期値を代入（時間変化あり）
- float f[36] = {
- 1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f,
- -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f,
- raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.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, 1.0f, 0.0f,
- 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
- };
-
- F.SetComps(f);
-
- // 観測方程式のヤコビアンの値を設定（時間変化無し）
- float h[18] = {
- 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
- 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,
- 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f
- };
-
- H.SetComps(h);
-}
-
-void KalmanUpdate() {
- // ヤコビアンの更新
- float f[36] = {
- 1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f,
- -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f,
- raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.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, 1.0f, 0.0f,
- 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
- };
-
- F.SetComps(f);
-
- // 事前推定値の更新
- pri_x = F * post_x;
- // 事前誤差分散行列の更新
- pri_P = F * post_P * F.Transpose() + R;
-
- // カルマンゲインの計算
- S = Q + H * pri_P * H.Transpose();
- float det;
- if((det = S.Inverse(inv)) >= 0.0f) {
- pc.printf("E:%.3f\r\n", det);
- return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
- }
- K = pri_P * H.Transpose() * inv;
-
- // 事後推定値の更新
- post_x = pri_x + K * (raw_geomag - H * pri_x);
- // 事後誤差分散行列の更新
- post_P = (I - K * H) * pri_P;
-}
-
-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;
- }
- //pc.printf("%d(us)\r\n", timer.read_us());
-
- if(INT_flag != FALSE) {
- g = raw_g;
- for(int i=0; i<3; i++) {
- geomag.SetComp(i+1, post_x.GetComp(i+1));
- }
- acc = raw_acc;
- gyro = raw_gyro;
- press = raw_press;
-
- }
-}
-
-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) {
-
- 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