Diff: main.cpp

Revision:

11:c90db1500720

Parent:

10:1a626929850e

Child:

12:6a3c0e9075eb

--- a/main.cpp	Sun Mar 10 03:41:56 2019 +0000
+++ b/main.cpp	Sun Mar 10 08:17:44 2019 +0000
@@ -1,66 +1,46 @@
 #include "mbed.h"
 #include "MPU6050.h"
 #include "BMP180.h"
-#include "Kalman.h"
-#include "MadgwickAHRS.h"
 
 /*しきい値など*/
-#define ACC_JUDGE_LAUNCH    3.0 //発射判定のしきい値
+#define ACC_JUDGE_LAUNCH    3.0 //発射判定の合成加速度のしきい値
 #define TIME_BURNING        6   //開放判定しない時間（燃焼時間）
-#define ALT_JUDGE_FIRE      0
 #define ALT_JUDGE_OPEN      1   //落下判定のカウントを１増やす高度差
 #define TIME_OPEN           25  //強制的に開放させる時間
-#define TIME_SEND           1.0
-#define ANGLE_JUDGE_FIRE_MIN    15
-#define ANGLE_JUDGE_FIRE_MAX    70
-#define CNT_JUDGE           10 
-#define TIME_JUDGE_CNT      1.5
-#define NUM_CNT_MEDIAN      10
-#define RATE_GPS            1.0
-#define RATE_DATA           10
-#define TIMER_NOTFIRE       15.0
-#define p0                  1013.25f
-#define RadToDeg            57.295779513082320876798154814105
+#define TIME_SEND           1.0 //無線送信する間隔
+#define CNT_JUDGE           10  //頂点判定する時に落下のカウント数
+#define NUM_CNT_MEDIAN      10  //中央値をとる個数
+#define RATE_GPS            1.0 //GPSのTickerを動作させるタイミング
+#define p0                  1013.25f //海面気圧
         
 MPU6050 mpu(PB_7,PB_6);
 BMP180  bmp(PB_7,PB_6);
-KalmanFilter gKfx, gKfy; 
-Madgwick MadgwickFilter;
 Serial pc(PA_2, PA_3);
 Serial gps(PA_9, PA_10);
 DigitalOut myled(PA_15);
 DigitalOut ES920_RST(PA_5);
+PwmOut     servo1(PB_4);
+PwmOut     servo2(PB_5);
+DigitalOut servo1_signal(PA_0);
+DigitalOut servo2_signal(PA_1);
 Timer timer_open;
 Timer timer_data;
-Ticker tic_data;
 Ticker tic_gps;
 
 /*自作関数*/
 float _getAlt();
 float _median(float data[],int num);
-void  _SendData();
 void  _SendGPS();
 float _DMS2DEG(float raw_data);
-int servo();
 
 enum PHASE{STANDBY=0,LAUNCH=1,RISE=3,FIRE=7,OPEN=15,RECOVERY=9,SEA=6} Phase;
 
 /*グローバル変数*/
-float t = 0;
 //地上高度
 float alt_gnd;
 float alt_max;
-//GPS
-int cnt_gps=0;
-
-int p = 1;
-//サーボ
+char c[3];
 int i = 0;
-char c[516];
-char d [8];
-int len;
-char f[]="Receive";
-
 void main(){
     ES920_RST = 0;
     myled =0;
@@ -69,7 +49,7 @@
     float alt_buff[10],alt_md;
     float time_judge;
     int cnt_data=0,cnt_judge=0;
-    char gps_data[256];
+    float t = 0;
     /*センサの初期化等*/
     pc.baud(38400);
     mpu.setAcceleroRange(3);
@@ -90,17 +70,35 @@
     pc.printf("f:Flight_mode_on\r\n");
     wait(1.0);
     timer_data.start();
+    servo1_signal = 1;
+    servo2_signal = 1;
+    servo1.pulsewidth(0.0024);
+    servo1.pulsewidth(0.0024);
     while(1){
         switch(Phase){
             case STANDBY:
                 /*サーボ入力待ち*/
                 //servo();
                 c[0]=pc.getc();
-                if(c[0] == 'o') myled = 1;
-                if(c[0] == 'l') myled = 0;
+                if(c[0] == 'o'){
+                    myled = 1;
+                    servo1.pulsewidth(0.0006);
+                    servo2.pulsewidth(0.0006);
+                    mpu.getAccelero(acc);
+                    pc.printf("OPEN:%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0)));
+                }
+                if(c[0] == 'l'){
+                    myled = 0;
+                    servo1.pulsewidth(0.0024);
+                    servo2.pulsewidth(0.0024);
+                    mpu.getAccelero(acc);
+                    pc.printf("LOCK:%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0)));
+                }
                 if(c[0] == 'f'){
                 //pc.printf("flight_mode\r\n");
                 Phase = LAUNCH;
+                servo1_signal = 0;
+                servo2_signal = 0;
                 }
                 break;
                 
@@ -119,6 +117,8 @@
                     cnt_judge++;
                 }
                 if(cnt_judge==CNT_JUDGE){
+                    servo1_signal = 1;
+                    servo1_signal = 1;
                     cnt_judge=0;
                     timer_open.start();
                     Phase = RISE;
@@ -136,62 +136,6 @@
                 t = 0.0;
                 break;
             case FIRE:
-
-                //カルマン
-                /*
-                gPrevMicros = timer_open.read();
-                mpu.getAccelero(acc);
-                mpu.getGyro(gyro);
-                degRoll  = atan2(acc[1], acc[2]) * RadToDeg;
-                degPitch = atan(-acc[0] / sqrt(acc[1] * acc[1] + acc[2] * acc[2])) * RadToDeg;
-                
-                float dpsX = gyro[0] * RadToDeg;
-                float dpsY = gyro[1] * RadToDeg;
-                float dpsZ = gyro[2] * RadToDeg;
-                
-                float curMicros = timer_open.read();
-                float dt = curMicros - gPrevMicros;
-                gPrevMicros = curMicros;
-
-                float degX = gKfx.calcAngle(degRoll, dpsX, dt);
-                float degY = gKfy.calcAngle(degPitch, dpsY, dt);
-                degY -= gCalibrateY;
-                degX -= gCalibrateX;
-                if(degY>ANGLE_JUDGE_FIRE){
-                    Phase = OPEN;
-                }
-                */
-
-                //madgwick
-                float Roll,Pitch,Yaw;
-                MadgwickFilter.begin(2);
-                mpu.getAccelero(acc);
-                mpu.getGyro(gyro);
-                gyro[0] *= RadToDeg;
-                gyro[1] *= RadToDeg;
-                gyro[2] *= RadToDeg;
-
-                MadgwickFilter.updateIMU(gyro[0],gyro[1],gyro[2],acc[0],acc[1],acc[2]);
-                Roll  = MadgwickFilter.getRoll();
-                Pitch = MadgwickFilter.getPitch();
-                Yaw   = MadgwickFilter.getYaw();
-                i++;
-                if(i==400){
-                    //pc.printf("TIME:%f,Pitch:%f\r\n",timer_data.read(),Pitch);
-                    pc.printf("FIRE:Pitch:%f,Time%f\r\n",Pitch,timer_open.read());
-                    i=0;
-                }
-                /*if(Pitch>ANGLE_JUDGE_FIRE_MIN && Pitch < ANGLE_JUDGE_FIRE_MAX){
-                    Phase = OPEN;
-                    i=0;
-                }
-                */
-                
-                //if(timer_open.read()>TIMER_NOTFIRE){
-                if(timer_data.read()>60.0){
-                    Phase = OPEN;
-                    pc.printf("NOT_FIRE!!\r\n");
-                }
                 break;
             case OPEN:
                 for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
@@ -232,49 +176,6 @@
     return altitude;
 }
 
-void _SendData(){
-    float pretime,a[3],g[3],alt;
-    //カルマン
-    /*
-    pretime = timer_data.read();
-    mpu.getAccelero(a);
-    mpu.getGyro(g);
-    float degroll  = atan2(a[1], a[2]) * RadToDeg;
-    float degpitch = atan(-a[0] / sqrt(a[1] * a[1] + a[2] * a[2])) * RadToDeg;
-    
-    float dpsx = g[0] * RadToDeg;
-    float dpsy = g[1] * RadToDeg;
-    float dpsz = g[2] * RadToDeg;
-    
-    float curtime = timer_data.read();
-    float t = curtime - pretime;
-    pretime = curtime;
-
-    float degx = gKfx.calcAngle(degroll, dpsx, t);
-    float degy = gKfy.calcAngle(degpitch, dpsx, t);
-    degy -= gCalibrateY;
-    degx -= gCalibrateX;
-    */
-    /*madgwick*/
-    float Roll,Pitch,Yaw;
-    MadgwickFilter.begin(2);
-    mpu.getAccelero(a);
-    mpu.getGyro(g);
-    g[0] *= RadToDeg;
-    g[1] *= RadToDeg;
-    g[2] *= RadToDeg;
-
-    MadgwickFilter.updateIMU(g[0],g[1],g[2],a[0],a[1],a[2]);
-    Roll  = MadgwickFilter.getRoll();
-    Pitch = MadgwickFilter.getPitch();
-    Yaw   = MadgwickFilter.getYaw();
-
-    alt = _getAlt();
-    alt = alt - alt_gnd;
-    //pc.printf("%d,%f,%f,%f\r\n",Phase,alt,degx,degy);
-    pc.printf("%d,%f,%f,%f\r\n",Phase,alt,Roll,Pitch);
-}
-
 float _DMS2DEG(float raw_data){
     int d=(int)(raw_data/100);
     float m=(raw_data-(float)d*100);
@@ -303,48 +204,9 @@
     return ans;
 }
 
-int servo(void){
-    do{
-        c[i] = pc.getc();
-        gps.printf("%c",c[i]);
-        i++;
-    }while(c[i-1] != '\n');
-    gps.printf("path\r\n");
-    //gps.printf("%s",c);
-    myled = 1;
-    //gps.printf("%s",c);
-    len = strlen(f);
-    i=0;
-    while(i != 4){
-        if (c[i] != f[i]) break;
-        i++;
-    }
-    if(i > 3){
-        i=0;
-        do{
-            d[i]=c[13+i];
-            i++;
-        }while(d[i-1] != ')');
-        d[i-1] = '\0';
-        gps.printf("d:%s",d);
-        i = 0;
-        if(d[0] == 'o') myled = 1;
-        if(d[0] == 'l') myled = 0;
-        if(d[0] == 'f'){
-        //pc.printf("flight_mode\r\n");
-        Phase = LAUNCH;
-        }
-    }
-        
-    i=0;
-    memset(c,'\0',64);
-    memset(d,'\0',8);
-    //pc.printf("2\r\n");
-    return 0;
-}
-
 void _SendGPS(){
     char gps_data[256];
+    int cnt_gps=0;
     while(1){
         if(gps.readable()){
             gps_data[cnt_gps] = gps.getc();