![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
v1
Diff: main.cpp
- Revision:
- 2:41238982b498
- Parent:
- 1:69472741bd36
- Child:
- 3:8e2d7ebdafb7
diff -r 69472741bd36 -r 41238982b498 main.cpp --- 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");