hiroya taura
MPU6050のサンプルプログラム２
Dependencies: ConfigFile SDFileSystem mbed
Fork of
LAURUS_program
by 
LAURUS
Diff: main.cpp
- Revision:
- 25:4c72d7420d8a
- Parent:
- 24:8838be99cec3
- Child:
- 26:6e09df57ee91
--- a/main.cpp Thu Jun 25 17:40:18 2015 +0000
+++ b/main.cpp Fri Jun 26 15:09:01 2015 +0000
@@ -12,20 +12,16 @@
#include "ConfigFile.h"
/****************************** private define ******************************/
-#define LEFT 0
-#define RIGHT 1
-#define NEUTRAL 2
-
#define RULE1
//#define RULE2
//#define RULE3
//#define SERVO_DEBUG
//#define DIRECTION_DEBUG
+
+
#ifdef DIRECTION_DEBUG
const float TargetDirection = 90.0f; // 真西に飛ぶ
#endif
-
-
const float dt = 0.01f; // 割り込み周期(s)
const float ServoMax = 0.0046f; // サーボの最大パルス長(s)
const float ServoMin = 0.0012f; // サーボの最小パルス長(s)
@@ -40,6 +36,8 @@
const float Beta = 60.0f; // 目標方向と自分の進行方向との間に取るべき角度差(deg)（制御則3の定数
const float BorderDistance = 10.0f; // 落下制御に入るための目標値との距離の閾値(m)（制御則2の定数
+enum Direction {LEFT, RIGHT, NEUTRAL};
+
/****************************** private macro ******************************/
/****************************** private typedef ******************************/
/****************************** private variables ******************************/
@@ -62,9 +60,10 @@
DigitalIn paraSensor(PB_0); // パラフォイルに繋がる（予定）の物理スイッチ
Ticker ticker; // 割り込みタイマー
Timer timer; // 時間計測用タイマー
+Direction dir = NEUTRAL; // 旋回方向
int lps_cnt = 0; // 気圧センサ読み取りカウント
-bool INT_flag = true; // 割り込み可否フラグ
+bool INT_flag = true; // 割り込み可否フラグ
/* こちらの変数群はメインループでは参照しない */
Vector raw_acc(3); // 加速度(m/s^2) 生
Vector raw_gyro(3); // 角速度(deg/s) 生
@@ -83,7 +82,8 @@
Vector raw_g(3); // 重力ベクトル 生
Vector g(3); // 重力ベクトル
Vector target_p(2); // 目標情報（経度、緯度）（rad）
-Vector p(2); // 位置情報(経度, 緯度)（rad）
+Vector p(2); // 現在の位置情報(補完含む)(経度, 緯度)（rad）
+Vector new_p(2); // 最新の位置情報(経度, 緯度)（rad）
Vector pre_p(2); // 過去の位置情報(経度, 緯度)（rad）
int UTC_t = 0; // UTC時刻
int pre_UTC_t = 0; // 前のUTC時刻
@@ -107,9 +107,10 @@
int step = 0; // シーケンス制御のステップ
int count = 0; // 安定滑空までの時間測定用カウンター
-int dir = 0; // 旋回方向（0：左 1：右）
char data[512] = {}; // 送信データ用配
int loopTime = 0; // 1ループに掛かる時間（デバッグ用
+float sv = 0.0f; // サーボ電源電圧
+float lv = 0.0f; // ロジック電源電圧
/** config.txt **
* #から始めるのはコメント行
@@ -161,13 +162,24 @@
void toString(Matrix& m); // デバッグ用
void toString(Vector& v); // デバッグ用
+/** 小さい値を返す関数
+ * @param a: 1つ目の入力値
+ * @param b: 2つ目の入力値
+ *
+ * @return a,bのうち、小さい方の値
+ */
inline float min(float a, float b)
{
return ((a > b) ? b : a);
}
-inline float Height(float press, float temp) {
- return (153.8f * (pow((p0/press),0.1902f)-1.0f)*(temp+273.15f));
+/** 気圧の変化から高度を計算する関数
+ * @param press: 現在の気圧
+ *
+ * @return height: 現在の基準点からの高度
+ */
+inline float Height(float press) {
+ return (153.8f * (pow((p0/press),0.1902f)-1.0f)*(25.0f+273.15f));
}
/****************************** main function ******************************/
@@ -230,8 +242,8 @@
ControlRoutine();
#endif
- float sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // サーボ電源電圧
- float lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // ロジック電源電圧
+ sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // サーボ電源電圧
+ lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // ロジック電源電圧
// 指定された引っ張り量だけ紐を引っ張る
if(pull_L < 0) pull_L = 0;
@@ -243,12 +255,11 @@
servoR.pulsewidth((ServoMax - ServoMin) * pull_R / (float)PullMax + ServoMin);
// データをmicroSDに保存し、XBeeでPCへ送信する
- sprintf(data, "%d, %.1f,%.1f,%.1f, %.3f,%.5f,%.5f, %.3f,%.3f,%.3f, %.3f,%d, %d,%d\r\n",
+ sprintf(data, "%d, %.1f,%.1f,%.1f, %.3f,%.5f,%.5f, %.3f,%.3f,%.1f, %d, %d,%d\r\n",
UTC_t, yaw, pitch, roll,
press, gms.longitude, gms.latitude,
- vrt_acc, temp, height,
- Distance(target_p, p), optSensor.read_u16(),
- pull_R, pull_L);
+ vrt_acc, height, Distance(target_p, p),
+ optSensor.read_u16(), pull_R, pull_L);
INT_flag = true; // 割り込み許可
@@ -274,6 +285,13 @@
*/
void DataProcessing()
{
+ static float R_11;
+ static float R_12;
+ static float r_cos;
+ static float r_sin;
+ static float p_cos;
+ static float p_sin;
+
gms.read(); // GPSデータ取得
UTC_t = (int)gms.time;
@@ -286,35 +304,46 @@
// オイラー角はヨー・ピッチ・ロールの順に回転したものとする
// 各オイラー角を求めるのに回転行列の全成分は要らないので、一部だけ計算する。
- 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)成分
+ R_11 = r_f * b_f; // 回転行列の(1,1)成分
+ R_12 = r_f * b_l; // 回転行列の(1,2)成分
#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;
+ r_cos = g.GetPerpCompTo(b_f).Normalize() * b_u;
+ r_sin = Cross(g.GetPerpCompTo(b_f).Normalize(), b_u) * b_f;
+ if(r_sin > 0.0f) {
+ roll = acos(r_cos) * RAD_TO_DEG;
+ } else {
+ roll = -acos(r_cos) * RAD_TO_DEG;
+ }
+
+ p_cos = g.GetPerpCompTo(b_l).Normalize() * b_u;
+ p_sin = Cross(g.GetPerpCompTo(b_l).Normalize(), b_u) * b_l;
+ if(p_sin > 0.0f) {
+ pitch = acos(p_cos) * RAD_TO_DEG;
+ } else {
+ pitch = -acos(p_cos) * 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);
-
+ pre_p = new_p;
+ new_p.SetComp(1, gms.longitude * DEG_TO_RAD);
+ new_p.SetComp(2, gms.latitude * DEG_TO_RAD);
+ p = new_p;
+ pre_UTC_t = UTC_t;
+
} else { // 更新されていなかったら
- // GPSの補完処理を追加予定
+ p += 0.2f * (new_p - pre_p);
}
- height = Height(press, temp);
+ height = Height(press);
- pre_p = p;
- pre_UTC_t = UTC_t;
}
@@ -324,6 +353,13 @@
*/
void ControlRoutine()
{
+ static float target_lng;
+ static float target_lat;
+ static float target_X;
+ static float target_Y;
+ static float theta;
+ static float delta_theta;
+
switch(step) {
// 投下&安定滑空シーケンス
@@ -355,13 +391,13 @@
} else {
/* いずれも地球を完全球体と仮定 */
- float target_lng = target_x * DEG_TO_RAD;
- float target_lat = target_y * DEG_TO_RAD;
+ target_lng = target_x * DEG_TO_RAD;
+ 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 ) * RAD_TO_DEG;
- float delta_theta = 0.0f;
+ target_Y = cos( target_lat ) * sin( target_lng - p.GetComp(1) );
+ target_X = cos( p.GetComp(2) ) * sin( target_lat ) - sin( p.GetComp(2) ) * cos( target_lat ) * cos( target_lng - p.GetComp(1) );
+ theta = -atan2f( target_Y, target_X ) * RAD_TO_DEG;
+ delta_theta = 0.0f;
#ifdef DIRECTION_DEBUG
theta = TargetDirection;
#endif
@@ -467,6 +503,7 @@
p0 = 0;
for(int i=0; i<10; i++) {
p0 += (float)lps.pressure() * PRES_LSB_TO_HPA;
+ wait(0.1f);
}
p0 /= 10.0f;
}
@@ -690,15 +727,23 @@
*/
float Distance(Vector p1, Vector p2)
{
+ static float mu_y;
+ static float s_mu_y;
+ static float w;
+ static float m;
+ static float n;
+ static float d1;
+ static float d2;
+
if(p1.GetDim() != p2.GetDim()) return 0.0f;
- float mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f;
- float s_mu_y = sin(mu_y);
- float w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y);
- float m = GPS_A * (1 - GPS_SQ_E) / (w * w * w);
- float n = GPS_A / w;
- float d1 = m * (p1.GetComp(2) - p2.GetComp(2));
- float d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1));
+ mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f;
+ s_mu_y = sin(mu_y);
+ w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y);
+ m = GPS_A * (1 - GPS_SQ_E) / (w * w * w);
+ n = GPS_A / w;
+ d1 = m * (p1.GetComp(2) - p2.GetComp(2));
+ d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1));
return sqrt(d1 * d1 + d2 * d2);
}