CORE
ヌペリオン星人の偵察型UAV「ヴェネサス」．その飛行アルゴリズムを解析し，C言語にて再現したもの．
Dependencies: MPU6050_2 MS5607 SDFileSystem mbed
Fork of
00NUPERION_ver12
by 
naoki nishii
Diff: main.cpp
- Revision:
- 0:98c7c6fe50cc
- Child:
- 1:491e67888f28
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Mar 16 15:06:44 2017 +0000
@@ -0,0 +1,330 @@
+#include "mbed.h"
+#include "MS5607I2C.h"
+#include "SDFileSystem.h"
+#include "MPU6050.h"
+#include <stdio.h>
+
+/***************************/
+/* 打上前に値を確認！！！！！ */
+/***************************/
+#define L_HOKUI 35.546257 // 10進法 KBIC 3本の樹
+#define L_TOUKEI 139.671875
+#define LNCH_ACC 2 // 発射判定閾値[G]
+#define TRANS_TIME 50 // 打上から制御開始までの時間[sec/10]
+/***************************/
+
+#define DEG_TO_RAD(x) ( x * 0.01745329 )
+#define RAD_TO_DEG(x) ( x * 57.29578 )
+
+#define HIGH 1
+#define LOW 0
+
+#define STDBY_MODE 0 // 待機
+#define TRANS_MODE 1 // 輸送
+#define CTRL_MODE 2 // 制御
+
+#define GPS_RATE 10 // GPSの取得レート
+#define SAVE_RATE 50 // SDへの保存の間隔[sec/10]
+
+#define ACC_SSF 2048 //Sensitivity Scale Factor
+#define GYRO_SSF 16.4
+#define PRESS_INIT 10 // 基準気圧の平均を取る数
+
+#define KPL 2.0 // ラダーゲイン
+#define KPE 2.0 // エレベータゲイン
+
+#define GOAL_R 50 // 目標地点にこれだけ近づいたら制御方法変える
+#define DELTA_E_FULLUP 2000
+#define DELTA_E_NTRL 1500
+#define DELTA_E_FULLDOWN 1000
+#define DELTA_L_FULLR 2000
+#define DELTA_L_NTRL 1500
+#define DELTA_L_FULLL 1000
+
+#define PI 3.14159265358979 // π
+#define A_RADIUS 6378137.000 // 地球長半径 [m]
+#define B_RADIUS 6356752.314245 // 地球短半径 (WGS84)[m]
+#define ECC2 0.006694379990 // 離心率の二乗
+
+/* ペリフェラル宣言 */
+Serial pc(USBTX,USBRX);
+Serial gps(p9, p10);
+Serial twe(p13, p14);
+PwmOut eServo(p21); // ラダー用サーボ
+PwmOut lServo(p22); // エレベータ用サーボ
+DigitalOut camSw(p20); // カメラスイッチ
+SDFileSystem sd(p5, p6, p7, p8, "sd");
+MPU6050 mpu;
+MS5607I2C ms5607(p28, p27, true);
+BusOut leds(LED1, LED2, LED3, LED4);
+
+/* グローバル変数宣言 */
+int mode = STDBY_MODE;
+int cnt = 0;
+
+int gps_i; // 文字数，モード（センテンス終わりか？判定）
+char gps_data[256];
+float beta, alpha;
+float set_point_l, set_point_e;
+float delta_l=1500, delta_e=1500;
+
+FILE *fp;
+char all_data[256];
+
+/* プロトタイプ宣言 */
+bool Init();
+void getData();
+float getAlt(float press, float press0, float temp);
+//void filter_x(double dt, double C, double x_est[], double x_gps[], double vg_gps[]);
+void filter_alt(double dt, double C, double &alt_est, double alt_s, double vz);
+void filter_vz(double dt, double C, double &vz_est, double vz);
+
+bool Init()
+{
+ gps.baud(9600); // ボーレート
+ pc.baud(115200);
+ twe.baud(115200);
+ eServo.period_us(20000); // servo requires a 20ms period
+ lServo.period_us(20000);
+ lServo.pulsewidth_us(0);
+ eServo.pulsewidth_us(0);
+
+ gps.printf("$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29\r\n"); // GPRMCだけにする
+ gps.printf("$PMTK300,100,0,0,0,0*2C\r\n"); // position fixを100msecに（レート10spsにする）
+
+ mpu.initialize();
+ ms5607.printCoefficients();
+
+ pc.puts("Init finish!\n");
+ return true;
+}
+
+/******************/
+// GPSの取得周期（10SPS）に沿って，他のデータ取得等の処理を行う．
+/******************/
+
+void getData()
+{
+ char c = gps.getc();
+ if( c=='$' || gps_i == 256) {
+ gps_i = 0;
+ }
+ if((gps_data[gps_i]=c) != '\r') {
+ gps_i++;
+ } else {
+
+ gps_data[gps_i]='\0';
+
+ float w_time, hokui, tokei; // 時刻，北緯，東経
+ float vb, beta2; // 速度，真方位
+ char ns, ew, status; // NS，EW，その他
+ if( sscanf(gps_data, "$GPRMC,%f,%c,%f,%c,%f,%c,%f,%f,",&w_time,&status,&hokui,&ns,&tokei,&ew,&vb,&beta2) >= 1) {
+ //Longitude
+ double d_tokei= int(tokei/100);
+ double m_tokei= (tokei-d_tokei*100)/60;
+ double g_tokei= d_tokei+m_tokei; // 10進法に換算
+ //Latitude
+ double d_hokui= int(hokui/100);
+ double m_hokui= (hokui-d_hokui*100)/60;
+ double g_hokui= d_hokui+m_hokui;
+ /* ヒュベニの公式 */
+ double dy = (L_HOKUI-g_hokui)/180*PI; //ラジアン
+ double dx = (L_TOUKEI-g_tokei)/180*PI;
+ double yAve = (g_hokui+L_HOKUI)/2/180*PI;
+ double w = sqrt(1-ECC2*sin(yAve)*sin(yAve));
+ double m = A_RADIUS*(1-ECC2)/(w*w*w);
+ double n = A_RADIUS/w;
+ double distance = sqrt((dy*m)*(dy*m)+(dx*n*cos(yAve))*(dx*n*cos(yAve)));
+ double xDist = dx*n*cos(yAve);
+ double yDist = dy*m;
+
+ /* 高度 */
+ static float alt, alt0; //前の高度
+ static float press_arr[PRESS_INIT]; // 基準高度はこれの平均をとる
+ static float press0 = 0;
+ float temp = ms5607.getTemperature(); // 気圧
+ float press = ms5607.getPressure(); // 温度
+
+ /* 基準電圧の取得 */
+ if(mode == STDBY_MODE) {
+ for(int i=0; i<PRESS_INIT-1; i++) press_arr[i] = press_arr[i+1]; // 気圧データをシフト
+ press_arr[PRESS_INIT-1] = press;
+ }
+ if(mode == TRANS_MODE) { //発射直後，基準電圧を計算
+ if(cnt==0 && press0==0) {
+ for(int i=0; i<PRESS_INIT; i++) press0 += press_arr[i];
+ press0 /= PRESS_INIT;
+ }
+ }
+
+ double vz_calc = (alt - alt0) / 0.1;
+ alt0 = alt;
+ alt = getAlt(press, press0, temp); //高度の算出
+ double alt_est = 0.0, vz_est = 0.0;
+ float dt = 1/GPS_RATE;
+ filter_alt(dt, 0.1, alt_est, alt, vz_est);
+ filter_vz(dt, 0.01, vz_est, vz_calc);
+
+ /* 加速度 */
+ int16_t ax, ay, az; // mpu生データ
+ int16_t gx, gy, gz;
+ mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+ float fax = float(ax)/ACC_SSF; // mpu物理量に換算
+ float fay = float(ay)/ACC_SSF;
+ float faz = float(az)/ACC_SSF;
+ //float fgx = float(gx)/GYRO_SSF;
+ //float fgy =float(gy)/GYRO_SSF;
+ //float fgz = float(gz)/GYRO_SSF;
+ float resAcc = sqrt(fax*fax + fay*fay + faz*faz); //合成加速度
+
+ /* アングルオフ */
+ if(beta2 > 180) beta2 -= 360;
+ double beta3 = RAD_TO_DEG(atan2(yDist, xDist));
+ beta = beta2 - beta3;
+ if(beta > 180) beta -= 360;
+ if(beta <-180) beta += 360;
+
+ /* 降下角 */
+ alpha = -atan(vz_est/vb);
+
+ /* サーボ操作 P制御 */
+ if(distance <= GOAL_R){
+ // ラダー操舵
+ set_point_l = DELTA_L_NTRL + 500*(beta * KPL / 180);
+ if(set_point_l > DELTA_L_FULLR) set_point_l = DELTA_L_FULLR;
+ if(set_point_l < DELTA_L_FULLL) set_point_l = DELTA_L_FULLL;
+ if((set_point_l-delta_l)>=0){
+ delta_l += 100;
+ if((set_point_l-delta_l)<0) delta_l = set_point_l;
+ }else{
+ delta_l -= 100;
+ if((set_point_l-delta_l)>0) delta_l = set_point_l+1;
+ }
+
+ // エレベータ制御
+ set_point_e = DELTA_E_NTRL + 500*(alpha * KPE / 180);
+ if(set_point_e > DELTA_E_FULLUP) set_point_e = DELTA_E_FULLUP;
+ if(set_point_e < DELTA_E_FULLDOWN) set_point_e = DELTA_E_FULLDOWN;
+ if((set_point_e-delta_e)>=0){
+ delta_e += 100;
+ if((set_point_e-delta_e)<0) delta_e = set_point_e;
+ }else{
+ delta_e -= 100;
+ if((set_point_e-delta_e)>0) delta_e = set_point_e+1;
+ }
+ }else{
+ if(cnt%10) delta_e = DELTA_E_NTRL;
+ else delta_e = DELTA_E_FULLUP;
+ delta_l = DELTA_L_FULLR; // 要検討
+ }
+ lServo.pulsewidth_us(delta_l);
+ eServo.pulsewidth_us(delta_e);
+
+ /* 取得データの整理と出力 */
+ sprintf(all_data, "%6lf,%.2f,%.2f,%.2f,%d,%d\r\n",
+ distance, vb, beta, alt, cnt, mode);
+ pc.printf("%s", all_data);
+ if(!(cnt%10)) twe.printf("%.6f,%.6f,%.2f,%d\r\n", g_tokei, g_hokui, alt, cnt);
+ //pc.printf("%s\r\n",gps_data);
+ //pc.printf("%f,%f\r\n", delta_l, set_point_l);
+
+ /* カメラスイッチ操作 */
+ if(mode==TRANS_MODE){
+ if(cnt<=20) camSw = HIGH; // 2秒間長押しで電源オンの後，一回短押しで撮影開始
+ if(cnt>20 && cnt<=25) camSw = LOW;
+ if(cnt>25 && cnt<=30) camSw = HIGH;
+ if(cnt>30) camSw = LOW;
+ }
+
+ /* SDカードに保存 */
+ if((cnt%SAVE_RATE)==0) fp = fopen("/sd/myfile.txt", "a");
+ fprintf(fp, "%s", all_data);
+ if((cnt%SAVE_RATE)==SAVE_RATE-1) fclose(fp);
+ sprintf(gps_data, "");
+
+ /* カウント */
+ cnt++;
+ if(cnt>=600) cnt = 0;
+
+ /* モード遷移判定 */
+ if(mode == STDBY_MODE) { // 待機⇒制御
+ if(resAcc > LNCH_ACC) {
+ mode = TRANS_MODE;
+ cnt = 0;
+ }
+ leds = 1 << cnt%4;
+ }else
+ if(mode == TRANS_MODE) {
+ if(cnt > TRANS_TIME) {
+ mode = CTRL_MODE;
+ cnt = 0;
+ }
+ leds = 3 << cnt%4;
+ }else
+ if(mode == CTRL_MODE) {
+ leds = 15*(cnt%2);
+ }
+ }
+ }
+}
+
+int main()
+{
+ Init();
+ pc.printf("*** Start up! ***\r\n");
+ gps.attach(getData,Serial::RxIrq);
+ while(1) {}
+}
+// 高度計算
+float getAlt(float press, float press0, float temp)
+{
+ return (pow((press0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
+}
+
+// 位置（速度）フィルタ
+void filter_x(double dt, double C, double x_est[], double x_gps[], double vg_gps[])
+{
+ static double x0[] = { x_gps[0], x_gps[1], x_gps[2] };
+ static int called = 0;
+ if (!called) {
+ for (int i = 0; i < 2; i++)
+ x_est[i] = x0[i];
+ called++;
+ return;
+ }
+ double xt[2];
+ for (int i = 0; i < 2; i++) {
+ xt[i] = x0[i] + vg_gps[i] * dt;
+ x_est[i] = C*x_gps[i] + (1 - C)*xt[i];
+ x0[i] = x_est[i];
+ }
+}
+// 高度フィルタ
+void filter_alt(double dt, double C, double &alt_est, double alt_s, double vz)
+{
+ static double alt0 = alt_s;
+ static int called = 0;
+ if (!called) {
+ alt_est = alt0;
+ called++;
+ return;
+ }
+ double altt = alt0 + vz * dt;
+ {
+ alt_est = C*alt_s + (1 - C)*altt;
+ alt0 = alt_est;
+ }
+}
+// 垂直速度フィルタ
+void filter_vz(double dt, double C, double &vz_est, double vz)
+{
+ static double vz0 = 0.0;
+ static int called = 0;
+ if (!called) {
+ vz_est = vz0;
+ called++;
+ return;
+ }
+ vz_est = C*vz + (1 - C)*vz0;
+ vz0 = vz_est;
+}