Hybrid_OB2021
/
Hybrid_OB2021_GROUND
v1
main.cpp
- Committer:
- imadaemi
- Date:
- 2021-10-19
- Revision:
- 3:8e2d7ebdafb7
- Parent:
- 2:41238982b498
File content as of revision 3:8e2d7ebdafb7:
#include "mbed.h" #include "IM920.h" #define RESET_TIME 1800 #define TEMP_MULTIPLIER 100 #define LPF_ALT_MULTIPLIER 100 #define LPF_VEL_MULTIPLIER 100 // *************************************************** // コンストラクタ // *************************************************** Serial pc(USBTX, USBRX, 115200); Serial im920_serial(D1, D0, 115200); IM920 im920(im920_serial, pc, 115200); // *************************************************** // 関数の宣言 // *************************************************** void ReadPC(); void ReadSensorSetup(); void ReadData(); void ReadLaunchTime(); void Read0xA5(); void Help(); // *************************************************** // 変数の宣言 // *************************************************** 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, lpf_altitude; float lpf_velocity; float imu[6], mag[3]; float mag_geo[3]; float voltage_main, voltage_sep; float current_main, current_sep; // *************************************************** // 無線のヘッダまとめ // *************************************************** const char HEADER_SETUP = 0x01; // 0x01, GPS, LPS22HB, MPU9250, MPU9250_MAG, INA226_MAIN, INA226_SEP // 1 1 1 1 1 1 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_LAUNCH_TIME = 0x03; const char HEADER_ECHO = 0x05; // 0xA5,コマンド // 1 1 // *************************************************** // メイン関数 // *************************************************** int main() { //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); } void ReadPC(){ char c = pc.getc(); if(!header_set){ im920.header((char)0xF0); header_set = true; } 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'; im_buf_n ++; pc.printf("INPUT : 0\r\n"); break; case '1': im920.write((char)0x01); im_buf[im_buf_n] = '1'; im_buf_n ++; pc.printf("INPUT : 1\r\n"); break; case '?': Help(); break; case '\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; } } void ReadSensorSetup(){ pc.printf("\r\n"); pc.printf("****Sensor Setup****\r\n"); if(im920.data[1] == 1){ pc.printf("GPS : OK\r\n"); }else{ pc.printf("GPS : NG\r\n"); } if(im920.data[2] == 1){ pc.printf("LPS22HB : OK\r\n"); }else{ pc.printf("LPS22HB : NG\r\n"); } if(im920.data[3] == 1){ pc.printf("MPU9250 : OK\r\n"); }else{ pc.printf("MPU9250 : NG\r\n"); } if(im920.data[4] == 1){ pc.printf("MPU9250_MAG : OK\r\n"); }else{ pc.printf("MPU9250_MAG : NG\r\n"); } if(im920.data[5] == 1){ pc.printf("INA226_MAIN : OK\r\n"); }else{ pc.printf("INA226_MAIN : NG\r\n"); } if(im920.data[6] == 1){ pc.printf("INA226_SEP : OK\r\n"); }else{ pc.printf("INA226_SEP : NG\r\n"); } for(int i = 0; i < 20; i++){ pc.printf("*"); } pc.printf("\r\n"); } 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 : %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 : %.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 = (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("Current Sep : %.2f [A]\r\n",current_sep); for(int i = 0; i < 20; i++){ pc.printf("*"); } 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 + time_reset_counter_launch*30; 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("*"); } 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; case 0x00: pc.printf("STOP SEPARATE\r\n"); break; } for(int i = 0; i < 20; i++){ pc.printf("*"); } pc.printf("\r\n"); } 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"); pc.printf("********************\r\n\r\n"); }