albatross
MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of
LAURUS_program
by 
hiroya taura
Diff: main.cpp
- Revision:
- 20:00afba164688
- Parent:
- 19:ad8ff2de68f5
- Child:
- 21:d417708e84a8
diff -r ad8ff2de68f5 -r 00afba164688 main.cpp
--- a/main.cpp Mon Jun 22 15:23:37 2015 +0000
+++ b/main.cpp Mon Jun 22 18:17:16 2015 +0000
@@ -48,13 +48,15 @@
SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); // microSD
FILE * fp; // ログファイルのポインタ
BufferedSerial xbee(PA_9, PA_10, PC_1); // Xbee
+//Serial xbee(PA_9, PA_10); // XBee
+//DigitalIn cts(PC_1); // ctsピン
ConfigFile cfg; // ConfigFile
PwmOut servoL(PB_6), servoR(PC_7); // サーボ用PWM出力
AnalogIn optSensor(PC_0); // 照度センサ用アナログ入力
-AnalogIn servoVcc(PA_1); // バッテリー電圧監視用アナログ入力(サーボ用)
-AnalogIn logicVcc(PA_0); // バッテリー電圧監視用アナログ入力(ロジック用)
+AnalogIn servoVcc(PA_0); // バッテリー電圧監視用アナログ入力(サーボ用)
+AnalogIn logicVcc(PA_1); // バッテリー電圧監視用アナログ入力(ロジック用)
DigitalIn paraSensor(PB_0); // パラフォイルに繋がる（予定）の物理スイッチ
-Ticker INT_timer; // 割り込みタイマー
+Ticker ticker; // 割り込みタイマー
Timer timer; // 時間計測用タイマー
int lps_cnt = 0; // 気圧センサ読み取りカウント
@@ -96,9 +98,10 @@
float vrt_acc = 0.0f; // 鉛直方向の加速度成分（落下検知に使用）
int step = 0; // シーケンス制御のステップ
-int count = 0; // 時間測定用カウンター
+int count = 0; // 安定滑空までの時間測定用カウンター
int dir = 0; // 旋回方向（0：左 1：右）
-char data[512] = {}; // 送信データ用配列
+char data[512] = {}; // 送信データ用配
+int loopTime = 0; // 1ループに掛かる時間（デバッグ用
/** config.txt **
* #から始めるのはコメント行
@@ -141,10 +144,12 @@
void KalmanInit(); // カルマンフィルタ初期化
void LoadConfig(); // config読み取り
int Find_last(); // SDカード初期化用関数
+void DataProcessing(); // データ処理関数
+void ControlRoutine(); // 制御ルーチン関数
void KalmanUpdate(); // カルマンフィルタ更新
float Distance(Vector p1, Vector p2); // 緯度・経度の差から2点間の距離を算出(m)
-bool thrown(); // 投下されたかどうかを判断する
-void INT_func(); // 割り込み用関数
+bool Thrown(); // 投下されたかどうかを判断する
+void DataUpdate(); // データ取得&更新関数
void toString(Matrix& m); // デバッグ用
void toString(Vector& v); // デバッグ用
@@ -174,8 +179,9 @@
// 各種ベクトルの初期化
VectorsInit();
-
- INT_timer.attach(&INT_func, dt); // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
+
+ ticker.attach(&DataUpdate, dt); // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
+ NVIC_SetPriority(TIM5_IRQn, 5);
servoL.period(0.020f); // サーボの信号の周期は20ms
servoR.period(0.020f);
@@ -187,195 +193,213 @@
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)成分
-
-#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, "%d, %.1f,%.1f,%.1f, %.3f,%.5f,%.5f, %.3f,%.3f,%.3f, %.3f,%d\r\n",
- UTC_t, 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; // 割り込み許可
- }
-
+ DataProcessing();
/******************************* 制御ルーチン *******************************/
- {
-
- switch(step) {
-
- // 投下&安定滑空シーケンス
- case 0:
- if(thrown() || count != 0) {
- count++;
- // 投下直後に紐を引く場合はコメントアウトをはずす
- // pull_L = 15;
- // pull_R = 15;
- if(count >= WaitTime*5) {
- pull_L = 0;
- pull_R = 0;
- step = 1;
- }
- }
- break;
-
- // 制御シーケンス
- case 1:
- if(fabs(roll) > BorderSpiral) {
- // スパイラル回避行動
- if(roll > 0) {
- pull_L = PullMax;
- pull_R = 0;
- } else {
- pull_L = 0;
- pull_R = PullMax;
- }
- } else {
-
- /* いずれも地球を完全球体と仮定 */
- 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;
-
- }
- }
-
-#ifdef RULE2
- // 目標地点との距離が閾値以下だった場合、落下シーケンスへと移行する
- if(Distance(target_p, p) < BorderDistance) step = 2;
-#endif
-
- break;
-
-#ifdef RULE2
- // 落下シーケンス
- case 2:
- pull_L = PullMax;
- 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);
-
- }
+ ControlRoutine();
myled = 0; // LED is OFF
+ loopTime = timer.read_ms();
+
// ループはきっかり0.2秒ごと
while(timer.read_ms() < 200);
-
+
}
/* ------------------------------ ↑↑↑ ここまでメインループ ↑↑↑ ------------------------------ */
}
+/** データ処理関数
+ *
+ */
+void DataProcessing()
+{
+ 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)成分
+
+#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, "%d, %.1f,%.1f,%.1f, %.3f,%.5f,%.5f, %.3f,%.3f,%d, %.3f,%d\r\n",
+ UTC_t, yaw, pitch, roll,
+ press, gms.longitude, gms.latitude,
+ lv, vrt_acc, loopTime,
+ Distance(target_p, p), optSensor.read_u16());
+ fprintf(fp, data);
+ fflush(fp);
+ xbee.printf(data);
+ /*
+ int dataLength = strlen(data);
+ for(int i=0; i<dataLength; i++) {
+ while(cts);
+ xbee.putc(data[i]);
+ }
+ */
+ INT_flag = TRUE; // 割り込み許可
+
+}
+
+/** 制御ルーチン関数
+ *
+ */
+void ControlRoutine()
+{
+ switch(step) {
+
+ // 投下&安定滑空シーケンス
+ case 0:
+ if(Thrown() || count != 0) {
+ count++;
+ // 投下直後に紐を引く場合はコメントアウトをはずす
+ // pull_L = 15;
+ // pull_R = 15;
+ if(count >= WaitTime*5) {
+ pull_L = 0;
+ pull_R = 0;
+ step = 1;
+ }
+ }
+ break;
+
+ // 制御シーケンス
+ case 1:
+ if(fabs(roll) > BorderSpiral) {
+ // スパイラル回避行動
+ if(roll > 0) {
+ pull_L = PullMax;
+ pull_R = 0;
+ } else {
+ pull_L = 0;
+ pull_R = PullMax;
+ }
+ } else {
+
+ /* いずれも地球を完全球体と仮定 */
+ 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;
+
+ }
+ }
+
+#ifdef RULE2
+ // 目標地点との距離が閾値以下だった場合、落下シーケンスへと移行する
+ if(Distance(target_p, p) < BorderDistance) step = 2;
+#endif
+
+ break;
+
+#ifdef RULE2
+ // 落下シーケンス
+ case 2:
+ pull_L = PullMax;
+ 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);
+}
+
/** 各種ベクトルの初期化を行う関数
*
*/
-void VectorsInit() {
+void VectorsInit()
+{
//重力ベクトルの初期化
raw_g.SetComp(1, 0.0f);
raw_g.SetComp(2, 0.0f);
@@ -389,7 +413,7 @@
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);
@@ -398,7 +422,8 @@
/** マイクロSDカードの初期化を行う関数
*
*/
-void SD_Init() {
+void SD_Init()
+{
//SDカード初期化
char filename[15];
int n = Find_last();
@@ -625,7 +650,7 @@
* @return 投下されたかどうか（true=投下 false=未投下
*
*/
-bool thrown()
+bool Thrown()
{
static int opt_count = 0;
static int g_count = 0;
@@ -646,9 +671,10 @@
}
-/* ------------------------------ 割り込み関数 ------------------------------ */
-
-void INT_func()
+/** データ取得＆更新関数
+ *
+ */
+void DataUpdate()
{
// センサーの値を更新
mpu.read();
@@ -686,6 +712,8 @@
}
}
+/* ------------------------------ 割り込み関数 ------------------------------ */
+
/* ------------------------------ デバッグ用関数 ------------------------------ */
void toString(Matrix& m)