albatross
MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of
LAURUS_program
by 
hiroya taura
Revision 14:f85cb5340cb8, committed 2015-06-19
- Comitter:
- ojan
- Date:
- Fri Jun 19 15:56:52 2015 +0000
- Parent:
- 13:df1e8a650185
- Child:
- 15:d14d385d37e2
- Commit message:
- LAURUS_controller_scripts_v1.0
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Jun 19 01:08:35 2015 +0000
+++ b/main.cpp Fri Jun 19 15:56:52 2015 +0000
@@ -12,19 +12,25 @@
#include "BufferedSerial.h"
#include "ConfigFile.h"
-/********** private define **********/
+/****************************** private define ******************************/
#define TRUE 1
#define FALSE 0
-const float dt = 0.01f; // 割り込み周期(s)
-const float ServoMax = 0.0023f; // サーボの最大パルス長(s)
-const float ServoMin = 0.0006f; // サーボの最小パルス長(s)
-const float PullMax = 25.0f; // 引っ張れる紐の最大量(mm)
-/********** private macro **********/
+const float dt = 0.01f; // 割り込み周期(s)
+const float ServoMax = 0.0023f; // サーボの最大パルス長(s)
+const float ServoMin = 0.0006f; // サーボの最小パルス長(s)
+const float PullMax = 25.0f; // 引っ張れる紐の最大量(mm)
+const float BorderSpiral = 40.0f; // スパイラル検知角度
+const short BorderOpt = 30000; // 光センサーの閾値
+const float BorderGravity = 0.3f; // 無重力状態の閾値
+const int BorderParafoil = 0; // 物理スイッチのOFF出力
+const int MaxCount = 3; // 投下シグナルを何回連続で検知したら投下と判断するか（×0.2[s]）
+const int WaitTime = 1; // 投下後、安定するまで何秒滑空するか
+const float Alpha = 30.0f; // 目標方向と自分の進行方向との差の閾値（制御則1の定数
-/********** private typedef **********/
-
-/********** private variables **********/
+/****************************** private macro ******************************/
+/****************************** private typedef ******************************/
+/****************************** private variables ******************************/
DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力
I2C i2c(PB_9, PB_8); // I2Cポート
MPU6050 mpu(&i2c); // 加速度・角速度センサ
@@ -35,48 +41,55 @@
GMS6_CR6 gms(&gps, &pc); // GPS
SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); // microSD
BufferedSerial xbee(PA_9, PA_10, PC_1); // Xbee
-ConfigFile cfg; //ConfigFile
+ConfigFile cfg; // ConfigFile
PwmOut servoL(PB_6), servoR(PC_7); // サーボ用PWM出力
-AnalogIn optSensor(PC_0); // 照度センサ用アナログ入力
+AnalogIn optSensor(PC_0); // 照度センサ用アナログ入力
AnalogIn servoVcc(PA_0); // バッテリー電圧監視用アナログ入力(サーボ用)
AnalogIn logicVcc(PA_1); // バッテリー電圧監視用アナログ入力(ロジック用)
+DigitalIn paraSensor(PB_0); // パラフォイルに繋がる（予定）の物理スイッチ
Ticker INT_timer; // 割り込みタイマー
+Timer timer; // 時間計測用タイマー
-int lps_cnt = 0; // 気圧センサ読み取りカウント
+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_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 p(2); // 位置情報(経度, 緯度)
-Vector pre_p(2); // 過去の位置情報(経度, 緯度)
-int UTC_t = 0; // UTC時刻
-int pre_UTC_t = 0; // 前のUTC時刻
-//Vector n(3); // 地磁気ベクトル
+Vector raw_g(3); // 重力ベクトル 生
+Vector g(3); // 重力ベクトル
+Vector target_p(2); // 目標情報（経度、緯度）（rad）
+Vector p(2); // 位置情報(経度, 緯度)（rad）
+Vector pre_p(2); // 過去の位置情報(経度, 緯度)（rad）
+int UTC_t = 0; // UTC時刻
+int pre_UTC_t = 0; // 前のUTC時刻
-Vector b_f(3); // 機体座標に固定された、機体前方向きのベクトル（x軸）
-Vector b_u(3); // 機体座標に固定された、機体上方向きのベクトル（z軸）
-Vector b_l(3); // 機体座標に固定された、機体左方向きのベクトル（y軸）
+Vector b_f(3); // 機体座標に固定された、機体前方向きのベクトル（x軸）
+Vector b_u(3); // 機体座標に固定された、機体上方向きのベクトル（z軸）
+Vector b_l(3); // 機体座標に固定された、機体左方向きのベクトル（y軸）
-Vector r_f(3); // 参照座標に固定された、北向きのベクトル（X軸）
-Vector r_u(3); // 参照座標に固定された、上向きのベクトル（Z軸）
-Vector r_l(3); // 参照座標に固定された、西向きのベクトル（Y軸）
+Vector r_f(3); // 世界座標に固定された、北向きのベクトル（X軸）
+Vector r_u(3); // 世界座標に固定された、上向きのベクトル（Z軸）
+Vector r_l(3); // 世界座標に固定された、西向きのベクトル（Y軸）
+
+int pull_L = 0; // 左サーボの引っ張り量(mm：0~PullMax)
+int pull_R = 0; // 右サーボの引っ張り量(mm：0~PullMax)
-int pull_L = 0; // 左サーボの引っ張り量(mm：0~PullMax)
-int pull_R = 0; // 右サーボの引っ張り量(mm：0~PullMax)
+float yaw = 0.0f; // 本体のヨー角（deg）z軸周り
+float pitch = 0.0f; // 本体のピッチ角（deg）y軸周り
+float roll = 0.0f; // 本体のロール角（deg）x軸周り
-float yaw = 0.0f; // 本体のヨー角（deg）z軸周り
-float pitch = 0.0f; // 本体のピッチ角（deg）y軸周り
-float roll = 0.0f; // 本体のロール角（deg）x軸周り
+float vrt_acc = 0.0f; // 鉛直方向の加速度成分（落下検知に使用）
-float vrt_acc = 0.0f; // 鉛直方向の加速度成分（落下検知に使用）
+int step = 0; // シーケンス制御のステップ
+int count = 0; // 時間測定用カウンター
+int dir = 0; // 旋回方向（0：左 1：右）
+char data[512] = {}; // 送信データ用配列
/** config.txt **
* #から始めるのはコメント行
@@ -86,8 +99,9 @@
*/
float target_x, target_y;
-/* ----- Kalman Filter ----- */
+/* ---------- Kalman Filter ---------- */
// 地磁気ベクトル用
+// ジャイロのz軸周りのバイアスも推定
Vector pri_x1(7);
Matrix pri_P1(7, 7);
Vector post_x1(7);
@@ -99,7 +113,7 @@
Matrix S1(3, 3), S_inv1(3, 3);
// 重力ベクトル用
-// ジャイロのバイアスも同時に推定する
+// ジャイロのx軸、y軸周りのバイアスも同時に推定
Vector pri_x2(5);
Matrix pri_P2(5, 5);
Vector post_x2(5);
@@ -109,38 +123,40 @@
Matrix I2(5, 5);
Matrix K2(5, 3);
Matrix S2(3, 3), S_inv2(3, 3);
-/* ----- ------------- ----- */
+/* ---------- ------------- ---------- */
-Timer timer;
-
-char data[512] = {};
-/********** private functions **********/
-void LoadConfig(); // config読み取り
-int find_last(); // SDカード初期化用関数
-void KalmanInit(); // カルマンフィルタ初期化
-void KalmanUpdate(); // カルマンフィルタ更新
-float distance(Vector p1, Vector p2); // 緯度・経度の差から2点間の距離を算出(m)
-void INT_func(); // 割り込み用関数
-void toString(Matrix& m);
-void toString(Vector& v);
+/****************************** private functions ******************************/
+void LoadConfig(); // config読み取り
+int Find_last(); // SDカード初期化用関数
+void KalmanInit(); // カルマンフィルタ初期化
+void KalmanUpdate(); // カルマンフィルタ更新
+float Distance(Vector p1, Vector p2); // 緯度・経度の差から2点間の距離を算出(m)
+bool thrown(); // 投下されたかどうかを判断する
+void INT_func(); // 割り込み用関数
+void toString(Matrix& m); // デバッグ用
+void toString(Vector& v); // デバッグ用
-/********** main function **********/
+inline float min(float a, float b) {
+ return ((a > b) ? b : a);
+}
+
+/****************************** main function ******************************/
int main() {
- i2c.frequency(400000); // I2Cの通信速度を400kHzに設定
+ 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();
+ int n = Find_last();
if(n < 0){
pc.printf("Could not read a SD Card.\n");
return 0;
@@ -149,14 +165,14 @@
fp = fopen(filename, "w");
fprintf(fp, "log data\r\n");
xbee.printf("log data\r\n");
- */
- servoL.period(0.020f); // サーボの信号の周期は20ms
+
+ servoL.period(0.020f); // サーボの信号の周期は20ms
servoR.period(0.020f);
//カルマンフィルタ初期化
KalmanInit();
- INT_timer.attach(&INT_func, dt); // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
+ INT_timer.attach(&INT_func, dt); // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
//重力ベクトルの初期化
raw_g.SetComp(1, 0.0f);
@@ -172,79 +188,81 @@
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();
- // 0.1秒おきにセンサーの出力をpcへ出力
- myled = 1; // LED is ON
+ myled = 1; // LED is ON
- // データ処理
+ /******************************* データ処理 *******************************/
{
- INT_flag = FALSE; // 割り込みによる変数書き換えを阻止
+ 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)成分
+ 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;
roll = atan2(-R_23, R_33) * RAD_TO_DEG;
pitch = asin(R_13) * RAD_TO_DEG;
- pc.printf("%.3f, %.3f, %.3f\r\n",
- //geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3));
- //yaw, pitch, roll);
- post_x2.GetComp(4),
- post_x2.GetComp(5),
- post_x1.GetComp(7));
+ 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);
+ 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;
+ 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,%.3f,%d\r\n",
- g.GetComp(1), g.GetComp(2), g.GetComp(3),
- geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3),
+ 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, optSensor.read_u16());
- //fprintf(fp, data);
- //fflush(fp);
- //xbee.printf(data);
+ 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) > 40.0f) {
+ if(fabs(roll) > BorderSpiral) {
// スパイラル回避行動
if(roll > 0) {
pull_L = PullMax;
@@ -255,15 +273,80 @@
}
} else {
- }
+ 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;
- //pull_L = (pull_L+5)%PullMax;
- //pull_R = (pull_R+5)%PullMax;
- //pull_L = 0;
- //pull_R = 30;
-
-
-
+ // 制御ルーチン
+ 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;
+ } 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;
+ }
+ }
+ } 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;
+ }
+ }
+ }
+
+
+ if(dir == 0) { //目標は左方向
+
+ pull_L = 20;
+ pull_R = 0;
+
+ } else if (dir == 1) { //目標は右方向
+
+ pull_L = 0;
+ pull_R = 20;
+
+ }
+
+ break;
+ }
+
+ }
+
// 指定された引っ張り量だけ紐を引っ張る
if(pull_L < 0) pull_L = 0;
else if(pull_L > PullMax) pull_L = PullMax;
@@ -273,13 +356,11 @@
servoL.pulsewidth((ServoMax - ServoMin) * pull_L / PullMax + ServoMin);
servoR.pulsewidth((ServoMax - ServoMin) * pull_R / PullMax + ServoMin);
+
+
}
myled = 0; // LED is OFF
- /*pc.printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n",
- post_x.GetComp(4), post_x.GetComp(5), post_x.GetComp(6),
- yaw, pitch, roll,
- geomag.GetNorm());*/
// ループはきっかり0.2秒ごと
while(timer.read_ms() < 200);
@@ -287,8 +368,7 @@
}
- /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */
- //fclose(fp);
+ /* ------------------------------ ↑↑↑ ここまでメインループ ↑↑↑ ------------------------------ */
}
void LoadConfig(){
@@ -309,7 +389,7 @@
}
}
-int find_last() {
+int Find_last() {
int i, n = 0;
char c;
DIR *dp;
@@ -474,7 +554,7 @@
}
}
-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;
@@ -488,7 +568,32 @@
return sqrt(d1 * d1 + d2 * d2);
}
-/* -------------------- 割り込み関数 -------------------- */
+/** 投下を検知する関数
+ *
+ * @return 投下されたかどうか（true=投下 false=未投下
+ *
+*/
+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() {
// センサーの値を更新
@@ -501,7 +606,7 @@
raw_geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS);
}
- Vector delta_g = Cross(raw_gyro, raw_g); // Δg = ω × g
+ 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();
@@ -512,7 +617,6 @@
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;
@@ -528,7 +632,7 @@
}
}
-/* -------------------- デバッグ用関数 -------------------- */
+/* ------------------------------ デバッグ用関数 ------------------------------ */
void toString(Matrix& m) {