Revision 2:41238982b498, committed 2021-08-08

Comitter:

imadaemi

Date:

Sun Aug 08 11:26:29 2021 +0000

Parent:

1:69472741bd36

Child:

3:8e2d7ebdafb7

Commit message:

v3

Changed in this revision

--- a/main.cpp	Thu Jul 29 18:37:48 2021 +0000
+++ b/main.cpp	Sun Aug 08 11:26:29 2021 +0000
@@ -1,6 +1,12 @@
 #include "mbed.h"
 #include "IM920.h"
 
+#define RESET_TIME 1800
+
+#define TEMP_MULTIPLIER 100
+#define LPF_ALT_MULTIPLIER 100
+#define LPF_VEL_MULTIPLIER 100
+
 // ***************************************************
 // コンストラクタ
 // ***************************************************
@@ -15,20 +21,44 @@
 void ReadPC();
 void ReadSensorSetup();
 void ReadData();
+void ReadLaunchTime();
 void Read0xA5();
 void Help();
 
 // ***************************************************
 // 変数の宣言
 // ***************************************************
-char nich_status;
+int minute;
+int second;
+int launch_minute;
+int launch_second;
+int flight_minute = 0;
+int flight_second = 0;
+float flight_time = 0;
+float duration_flight_time = 0;
+float main_time;
+float duration_main_time = 0;
+float launch_time = 0;
+int time_reset_counter = 0;
+int time_reset_counter_launch = 0;
+
+char mode;
+
+bool top_detect;
+bool flightpin_status;
+bool nich_status;
+bool save_data_status;
+char bitshift_sum;
 
 bool header_set = false;
 char im_buf[16];//16なのって意味ある？
 int im_buf_n = 0;
 
+char eeprom_number;
+
 float lat, lon, height;
-float press, temp, altitude;
+float press, temp, lpf_altitude;
+float lpf_velocity;
 
 float imu[6], mag[3];
 float mag_geo[3];
@@ -40,13 +70,16 @@
 // 無線のヘッダまとめ
 // ***************************************************
 const char HEADER_SETUP = 0x01;
-// 0xA1
+// 0x01, GPS, LPS22HB, MPU9250, MPU9250_MAG, INA226_MAIN, INA226_SEP
+//   1    1       1        1          1           1            1     
 
-const char HEADER_DATA = 0xA2;
-// 0xA2,lat,lon,height,press,temp,altitude
-//   1 
+const char HEADER_DATA = 0x02;
+// 0xA2, duration_main_time, duration_flight_time, mode, nich_status, flightpin_status, lat, lon, height, press, temp, altitude, voltage_main, voltage_sep, current_main, current_sep
+//   1            4                      4           1         0.125         0.125       4    4      4      4      4       4            4             4            4             4
 
-const char HEADER_ECHO = 0xA5;
+const char HEADER_LAUNCH_TIME = 0x03;
+
+const char HEADER_ECHO = 0x05;
 // 0xA5,コマンド
 //   1     1
 
@@ -54,9 +87,10 @@
 // メイン関数
 // ***************************************************
 int main() {
-    wait(0.5f);
+    //wait(0.5f);
     pc.printf("Hello Ground!\r\n");
     pc.attach(&ReadPC, Serial::RxIrq);
+    im920.attach(&ReadLaunchTime, HEADER_LAUNCH_TIME);
     im920.attach(&Read0xA5, HEADER_ECHO);
     im920.attach(&ReadSensorSetup, HEADER_SETUP);
     im920.attach(&ReadData, HEADER_DATA);
@@ -70,6 +104,63 @@
     }
     
     switch(c){
+        case 'U':
+        im920.write((char)c);
+        im_buf[im_buf_n] = 'U';
+        im_buf_n ++;
+        pc.printf("INPUT : U\r\n");       
+        if(header_set){
+            im920.send();
+            pc.printf("Send : %s\r\n", im_buf);
+            header_set = false;
+            for(int i = 0; i < im_buf_n; i ++){
+                im_buf[i] = '\0';
+            }
+            im_buf_n = 0;
+        }
+        break;
+        
+        case 'O':
+        im920.write((char)c);
+        im_buf[im_buf_n] = 'O';
+        im_buf_n ++;
+        pc.printf("INPUT : O\r\n");
+        break;
+        
+        case 'F':
+        im920.write((char)c);
+        im_buf[im_buf_n] = 'F';
+        im_buf_n ++;
+        pc.printf("INPUT : F\r\n");
+        
+        if(header_set){
+            im920.send();
+            pc.printf("Send : %s\r\n", im_buf);
+            header_set = false;
+            for(int i = 0; i < im_buf_n; i ++){
+                im_buf[i] = '\0';
+            }
+            im_buf_n = 0;
+        }
+        break;
+        
+        case 'S':
+        im920.write((char)c);
+        im_buf[im_buf_n] = 'S';
+        im_buf_n ++;
+        pc.printf("INPUT : S\r\n");
+        
+        if(header_set){
+            im920.send();
+            pc.printf("Send : %s\r\n", im_buf);
+            header_set = false;
+            for(int i = 0; i < im_buf_n; i ++){
+                im_buf[i] = '\0';
+            }
+            im_buf_n = 0;
+        }
+        break;
+        
         case '0':
         im920.write((char)0x00);
         im_buf[im_buf_n] = '0';
@@ -146,44 +237,167 @@
 void ReadData(){
     //pc.printf("***Acquired Data****\r\n");
     pc.printf("***Recieved Data****\r\n");
+    
     int bit = 1;
+    
+    time_reset_counter = (short)im920.toShort(bit);
+    bit += 2;
+    
+    main_time = (float)im920.toFloat(bit);
+    main_time /= 1000;
+    bit += 4;
+    
+    minute = main_time / 60 + time_reset_counter*30;
+    //pc.printf("%d\t%f\r\n",minute,minute);
+    second = (int)main_time % 60;
+    
+    //duration_main_time = main_time + RESET_TIME*time_reset_counter;
+    pc.printf("Main Time       : %d : %02d\r\n",minute,second);
+    //pc.printf("Main Time      : %f\r\n",duration_main_time);
+    
+    pc.printf("Launch Time     : %d : %02d\r\n",launch_minute,launch_second);
+      
+    if(mode >= 0x04){
+            if(second >= launch_second){
+                flight_minute = minute - launch_minute;
+                flight_second = second - launch_second;
+            }else{
+                flight_minute = minute - launch_minute - 1;
+                flight_second = 60 + second - launch_second;
+            }
+            pc.printf("Flight Time     : %d : %02d\r\n",flight_minute,flight_second);
+    }else{
+        pc.printf("Flight Time     : %d : %02d\r\n",flight_minute,flight_second);   
+    }
+    
+    mode = im920.data[bit];
+    bit += 1;
+    switch(mode){
+        case 0x00:
+        pc.printf("Mode            : SETUP\r\n");
+        break;
+        
+        case 0x01:
+        pc.printf("Mode            : SAFETY\r\n");
+        break;
+        
+        case 0x02:
+        pc.printf("Mode            : TO_ONCALL\r\n");
+        break;
+        
+        case 0x03:
+        pc.printf("Mode            : ONCALL\r\n");
+        break;
+        
+        case 0x04:
+        pc.printf("Mode            : TO_FLIGHT\r\n");
+        break;
+        
+        case 0x05:
+        pc.printf("Mode            : FLIGHT\r\n");
+        break;
+        
+        case 0x06:
+        pc.printf("Mode            : TO_SEPARATE\r\n");
+        break;
+        
+        case 0x07:
+        pc.printf("Mode            : SEPARATE\r\n");
+        break;
+        
+        case 0x08:
+        pc.printf("Mode            : TO_DESCEND\r\n");
+        break;
+        
+        case 0x09:
+        pc.printf("Mode            : DESCEND\r\n");
+        break;
+        
+        case 0x0a:
+        pc.printf("Mode            : TO_COLLECTION\r\n");
+        break;
+        
+        case 0x0b:
+        pc.printf("Mode            : COLLECTION\r\n");
+        break;
+    }
+    
+    /*bool1個を1バイトで送る
+    flightpin_status = im920.data[bit];
+    bit += 1;        
+    pc.printf("FlightPin Status: %d\r\n",flightpin_status);
+    
     nich_status = im920.data[bit];
     bit += 1;
-    pc.printf("Nichrome Status : %c\r\n",nich_status);
+    pc.printf("Nichrome Status : %d\r\n",nich_status);
+    */
+    
+    //bool8個を1バイトで送る
+    bitshift_sum = im920.data[bit];
+    bit += 1;
+    //pc.printf("Bitshift Sum    : %d\r\n",bitshift_sum);
+    
+    flightpin_status = 0x01 & (bitshift_sum); 
+    pc.printf("FlightPin Status: %d\r\n",flightpin_status);
+    
+    //nich_status = bitshift_sum >> 1;
+    nich_status = 0x02 & (bitshift_sum);
+    pc.printf("Nichrome Status : %d\r\n",nich_status);
+    
+    top_detect = 0x04 & (bitshift_sum);
+    pc.printf("Top Detect      : %d\r\n",top_detect);
+    
+    save_data_status = 0x08 & (bitshift_sum);
+    pc.printf("Save Data Status: %d\r\n",save_data_status);
+    
+    eeprom_number = im920.data[bit];
+    bit += 1;
+    pc.printf("eeprom_number   : %d\r\n",eeprom_number);
+    
     lat = (float)im920.toFloat(bit);
     bit += 4;
     pc.printf("Latitude        : %.7f\r\n",lat);
     lon = (float)im920.toFloat(bit);
     bit += 4;
     pc.printf("Longitude       : %.7f\r\n",lon);
+    /*
     height = (float)im920.toFloat(bit);
     bit += 4;
     pc.printf("Height          : %.7f\r\n",height);
-    
+    */
     press = (float)im920.toFloat(bit);
     bit += 4;
-    pc.printf("Pressure        : %.4f[hPa]\r\n", press);
-    temp = (float)im920.toFloat(bit);
-    bit += 4;
-    pc.printf("Temperarure     : %.2f[℃]\r\n", temp);
-    altitude = (float)im920.toFloat(bit);
-    bit += 4;
-    pc.printf("Altitude        : %.2f[m]\r\n", altitude);
+    pc.printf("Pressure        : %.2f [hPa]\r\n", press);
+    temp = (short)im920.toShort(bit);
+    temp /= TEMP_MULTIPLIER;
+    bit += 2;
+    pc.printf("Temperarure     : %.2f [degC]\r\n", temp);
+    lpf_altitude = (short)im920.toShort(bit);
+    lpf_altitude /= LPF_ALT_MULTIPLIER;
+    bit += 2;
+    pc.printf("LPF Altitude    : %.2f [m]\r\n", lpf_altitude);
+    
+    lpf_velocity = (short)im920.toShort(bit);
+    lpf_velocity /= LPF_VEL_MULTIPLIER;
+    bit += 2;
+    pc.printf("Lpf Velocity    : %f [m/s]\r\n", lpf_velocity);
     
-    voltage_main = (float)im920.toFloat(bit);
+    voltage_main = (short)im920.toShort(bit);
+    voltage_main /= 1000;
+    bit += 2;
+    pc.printf("Voltage Main    : %.2f [V]\r\n",voltage_main);
+    current_main = (short)im920.toShort(bit);
+    current_main /= 1000;
+    bit += 2;
+    pc.printf("Crrent Main     : %.2f [A]\r\n",current_main);
+    voltage_sep = (short)im920.toShort(bit);
+    voltage_sep /= 1000;
+    bit += 2;
+    pc.printf("Voltage Sep     : %.2f [V]\r\n",voltage_sep);
+    current_sep = (short)im920.toShort(bit);
+    current_sep /= 1000;
     bit += 4;
-    pc.printf("Voltage Main    : %.2f\r\n",voltage_main);
-    current_main = (float)im920.toFloat(bit);
-    bit += 4;
-    pc.printf("Crrent Main     : %.2f\r\n",current_main);
-    voltage_sep = (float)im920.toFloat(bit);
-    bit += 4;
-    pc.printf("Voltage Sep     : %.2f\r\n",voltage_sep);
-    current_sep = (float)im920.toFloat(bit);
-    bit += 4;
-    pc.printf("Current Sep     : %.2f\r\n",current_sep);
-    
-    
+    pc.printf("Current Sep     : %.2f [A]\r\n",current_sep);
     
     for(int i = 0; i < 20; i++){
         pc.printf("*");
@@ -191,6 +405,19 @@
     pc.printf("\r\n\r\n");
 }
 
+void ReadLaunchTime(){
+    int bit = 1;
+    launch_time = (float)im920.toFloat(bit);
+    bit += 4;
+    launch_time /= 1000;
+    time_reset_counter_launch = (short)im920.toShort(bit);
+    bit += 2;
+    launch_minute = launch_time / 60;
+    launch_second = (int)launch_time % 60;
+    pc.printf("Get launch time\r\n");
+    pc.printf("launch time : %f\r\n",launch_time);
+}
+
 void Read0xA5(){
     for(int i = 0; i < 20; i++){
         pc.printf("*");
@@ -198,6 +425,30 @@
     pc.printf("\r\nIM920 ECHO : ");
     
     switch(im920.data[1]){
+        case 'U':
+        pc.printf("MODE SETUP\r\n");
+        break;
+        
+        case 'O':
+        pc.printf("MODE ONCALL\r\n");
+        break;
+        
+        case 'F':
+        pc.printf("MODE FLIGHT\r\n");
+        break;
+        
+        case 'S':
+        pc.printf("MODE SEPARATE\r\n");
+        break;
+        
+        case 'D':
+        pc.printf("MODE DESCEND\r\n");
+        break;
+        
+        case 'C':
+        pc.printf("MODE COLLECTION\r\n");
+        break;
+
         case 0x01:
         pc.printf("SEPARATE\r\n");
         break;
@@ -216,6 +467,12 @@
 void Help(){
     pc.printf("\r\n");
     pc.printf("**I can help you!***\r\n");
+    pc.printf("U : Mode Setup\r\n");
+    pc.printf("O : Mode Oncall\r\n");
+    pc.printf("F : Mode Flight\r\n");
+    pc.printf("S : Mode Separate\r\n");
+    pc.printf("D : Mode Descend\r\n");
+    pc.printf("C : Mode Collection\r\n");
     pc.printf("0 : Stop Separate\r\n");
     pc.printf("1 : Separate\r\n");
     pc.printf("? : Help\r\n");